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ABSTRACT 



DUDLEY KNOX LIBRARY 
NAVAL POSTGRADUATE SCHOOL 
MONTEREY CA 93S43-61U1 

Millions of acres of the US formerly used defense sites (FUDS) are contaminated 



with unexploded ordnance (UXO) as a result of past military use. The process of 
returning the land to the civilian sector is sensitive, intensive, and costly (e.g., millions of 
dollars, and the loss of human life). Hence “clearing” (i.e., site remediation, range 
clearance, and explosive ordnance disposal) UXO’s from FUDS is a complex problem. 
Existing clearing methods are inaccurate, dangerous, and labor intensive. This thesis 
shows that through robotics technology (e.g., “Shepherd” rotary vehicle with three 
degrees of freedom) and the use of advanced computer technology it is possible to make 
clearing tasks safer, more cost-effective, and more efficient. An over arching hardware 
and software architecture was developed for Shepherd (including a self-contained on- 
board computer system). The software system was developed for timer control, motion 
control, user interface, and an operating kernel. The hardware and software organization, 
structure, and interaction provide the framework for real-time control. This research 
included the use of encoders, digital boards, and a counter board; required the handling of 
interrupts, electric motor manipulation by servomotor controllers, and communication 
using RS232 and VMEbus technology. The kinematics algorithms and a real-time 
operating kernel were implemented using the C language. “Shepherd” research has laid 
the foundation for the flexible, robust, and precise motion needed for UXO clearing. 
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I. INTRODUCTION 



A. MOTIVATION AND BACKGROUND 

Land mines are an inexpensive and effective defensive means in wars. The 
problem with land mines is that they remain to be a threat when wars are over. 
International efforts are being made to ensure that land mines deployed in the future are 
equipped with a time-out device, and mine locations are properly recorded. While such a 
treaty may provide relief in the future, millions of land mines were planted all over the 
world as a result of wars and regional conflicts in the pzist. 

There are about 1 10 million land mines scattered around the world in more than 
60 countries — most of them in the Third World [Ref. 1, 2, 3, 4, and 5]. These land 
mines kill about 10,000 and injure another 20,000 people (many of them are children) 
every year. Moreover, there are millions of acres of the US formerly used defense sites 
(FUDS) that are contaminated with unexploded ordnance (UXO) as a result of military 
testing and training in the past [Ref. 6]. The contaminated land must be cleared inch by 
inch before transferring to civilian use. The difficulties of these clearing missions are in 
the variety of the objects to be identified and the diversity of the environments that are 
contaminated. 

B. OBJECTIVES 

As the military continues to downsize, the process of turning the land over to the 
civilian sector is sensitive, intensive, and costly. The aforementioned costs are both 
monetary and in some instance the loss of human life. One of the most complex 
problems is the clearing of UXO’s from the FUDS. 

The Department of Defense (DOD) has recently approved two organizational 
structures to confront the challenge of UXO remediation and wide-area de-mining. The 
objective of the first committee is to develop fully coordinated requirements driven 
research and development program for countermine, de-mining, site remediation, range 
clearance, and explosive ordnance disposal. Within the first committee there is a specific 
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group focused on detection technology. The second committee will focus on current 
technologies and ways to improve in the future. One of the phases will examine current 
UXO remediation, active range UXO clearance and explosive ordnance disposal efforts. 
Hence, the UXO problem is serious and a highly visible issue within the DOD. The 
current approach to mine and UXO clearing is dangerous and labor intensive [Ref. 6]. In 
a typical UXO clearing scenario. Explosive Ordnance Disposal (EOD) technicians walk 
slowly and carefully over a contaminated field in an attempt to identify the presence of 
UXO’s that may be fully buried, half buried, or totally on the surface. UXO’s found on 
the surface are visually examined to determine their types and fuse mechanisms. If fuse 
mechanisms are recognized and the condition of the UXO permits (e.g., not rusted, 
decayed, or encased in soil), an effort is made to defuse UXO’s. UXO’s that cannot be 
defused are gathered at a safe location and are destroyed using shaped charges. 
Transporting any live UXO is extremely dangerous and the motion of the transport 
vehicle must be gentle. Moreover, buried UXO’s must be unearthed first. Therefore, 
UXO clearing consists of detection, identification, defusing, excavating, transporting, and 
disposal. Through robotics and the use of advanced technology it should be possible to 
make UXO clearing tasks safer, cost-effective, and more efficient. 

At the Naval Postgraduate School a team has been put together to develop a 
semiautonomous vehicle or robot, which will survey possible contaminated areas for 
UXO’s. The “rotary” class vehicle by its very design is capable of independent driving 
and steering with each wheel. Hence, rotary vehicles are capable of stronger torque and 
traction than most other vehicles on rugged terrains. Rotary vehicles with two wheels (on 
a very smooth surface) have been shown to possess the aforementioned enhanced torque 
and traction - with an increase in the number of wheels on the vehicle the capabilities are 
significantly improved. The vehicle needs to be highly mobile and capable of producing 
very fine motion to negotiate and search contaminated sites. To fulfill the requirement of 
the special wheel architecture, the semiautonomous vehicle, called Shepherd (a rotary 
vehicle), is presently under development. Shepherd possesses stronger torque and 
traction on rugged terrain, because of its special architecture (i.e., useful for the 
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mineAJXO. Mission). The name “Shepherd” was given to the vehicle because of the 
protective function of the “shepherd” in many ancient cultures — hopefully this Shepherd 
will also save lives. Shepherd (Figure 1.1) is a four-wheeled vehicle with independent 
steering and driving capabilities. The four-wheel independent driving and steering 
capability provides Shepherd a high level of mobility and preciseness in motion control 
[Ref. 7]. 




Figure 1.1: A “rotary” vehicle: Shepherd. 

The fundamental objective of this thesis research project is to assist in the 
construction of a user-friendly and high-precision semiautonomous robotics tool to help 
the unexploded ordnance (UXO) and mine clearing mission. 
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This thesis will examine the following research areas; 

• What kinematics algorithms must be developed to support a vehicle with three 
degrees of freedom of motion? The aforementioned algorithms must support 
highly flexible, controlled, and precise motion. 

• What types of controls are required to ensure the optimal mix of driving and 
steering resources? Moreover, what must be done to ensure that all the 
resources complement? 

• How can the knowledge gained in the aforementioned research areas be used 
to develop searching motion? 

• How should the hardware and software systems be implemented to support 
the aforementioned goals? 

• How can human operators remotely control the vehicle through an appropriate 
interface? 



C. ORGANIZATION 

This chapter provides a general overview of traditional and current techniques for 
identifying unexploded ordnance. Chapter II provides the System Overview and 
illustrates the concept of motion control. Chapter III describes the Shepherd Mobile 
Platform and the On-Board Computer System. Chapter IV presents the Shepherd 
Software Description and places great emphasis on the Shepherd Real-time Kernel 
(SRK). In Chapter V the results of experiments and testing motion control are presented. 
Chapter VI explains the current motion modes and Chapter VII summarizes the thesis. 
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II. SHEPHERD SYSTEM DESIGN 



A. SYSTEM OVERVIEW 

In consideration of aiding the UXO task, what type of vehicle should be 
developed? This vehicle will face difficulties of clearing missions in the variety of the 
objects to be identified and the diversity of the environments that are contaminated. This 
vehicle has to have precise and smooth motion, display motion flexibility, and contain 
robust motion in varied environments such as soft soil and rough terrain. 

The vehicle must be capable of precise and smooth motion while searching for 
UXO’s. The very nature of trying to locate UXO’s should be meticulous and cautious. 
Haphazzard and Jerky motions could contribute to lost of the vehicle due to unwanted 
detonations. Also, motion flexibility is absolutely necessary. This will enable different 
approaches or techniques for locating UXO’s. Finally, the motion exhibited must be 
robust and stable due to the nature of UXO environments. While traversing these 
environments, the vehicle should not lose its precise, smooth, and flexible motion 
characteristics. For these reasons, a rigid body vehicle with at least 2 steerable wheels 
capable of semiautonomous or autonomous motions and equipped with sensors for 
detecting UXO’s was considered at the abstract level. 

So, the rotary vehicle platform was chosen, with the addition of four steerable and 
drivable wheels and a powerful computer system for control. The four wheels have thick 
tires and each contains two motors, one for driving and one for steering. Because of this, 
three degrees of freedom motion is possible which allows for motion flexibility. The 
independently driven four wheels aids in providing stronger traction than any other 
wheeled vehicle allowing for the negotiating of uneven slopes , soft soil, or rough terrain. 
The vehicle itself also provides the capability for further expansion of the system which 
will give it the full capability of fulfilling all aspects of the UXO mission. Chapters III 
and rv expand on the details of the system architecture regarding the hardware and 
software of this rotary vehicle. 
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B. MOTION MODES 



Due to the uniqueness of Shepherd’s independent 4-wheel motion of 360 degrees, 
several modes of motion are possible. The possible vehicle motions are: 

• Tangential — the vehicle’s change in direction of movement is equal to its 
change in heading. 

• Constant orientation — the vehicle’s heading is constant regardless of the 
change in the direction of movement. 

• Complex which falls into neither of the above. 

• Rotation 

At this stage of development of Shepherd the constant orientation, complex, and rotation 
motion modes have been implemented with work proceeding on the tangential motion 
mode. 

The user interface menu has a list of specific motions designed for the vehicle, 
which encompasses the typical vehicle motions. This list includes 1-Stop, 2-Straight 
motion (autonomous), 3-Straight motion by joystick, 4-XY-motion by Joystick, 5-Rotate, 
6-Sinusoidal, 7-Tomado (external), 8-Tomado (internal), 9-Tangential, 0-Exit, a- 
Tangential motion II, and t-Test motion. The sinusoidal motion is an implementation of 
the constant orientation while the tornado motions are an implementation of the complex 
vehicle motion. Both tangential motions are an attempt to implement the tangential 
vehicle motion. Further coverage will be given to several of these motions in more 
detail. 
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III. SHEPHERD SYSTEM HARDWARE 



A. OVERVIEW 

The Shepherd system hardware consists of the mobile platform and the shepherd 
on-board computer system. The mobile platform is the “mechanical” part of Shepherd, 
which provides motion and is directed by the on-board computer system. The Shepherd 
on-board computer system provides the computing power required controlling and 
directing Shepherd. Figure 3.1 provides a global perspective of the Shepherd System 
Hardware. Shepherd has four wheels, which are controlled independently. Each wheel 
has two motors; one for driving the wheel and the other for steering. Moreover, the 
steering capability for each wheel exceeds 360 degrees. The maximum driving speed 
(determined empirically) is approximately 87 centimeters/second. The unique mix of 
driving and steering capability is what provides the challenge and motion flexibility of 
this vehicle. 

Shepherd has a mass of 150 kilograms and is built to form a square chassis 
(frame). Shepherd’s wheels are centered on the comers of the square leading to very 
elegant calculations, as you will see later. Shepherd’s Alternating Current (AC) electric 
motors are powered by 12 batteries, which are charged by an external AC source through 
converter. Figure 3. 1 is also a transparent view of Shepherd from above which shows the 
Central Processor Unit (CPU), input/output (I/O) boards, servoamplifiers, batteries, and 
wheel unit assembly. 



B. THE MOBILE PLATFORM 

The mobile platform consists of the vehicle body. The vehicle body includes the 
vehicle’s frame, motors, encoders, servocontrollers, gears, wheels, tires, and power 
supply. The on-board computer system is not considered part of the mobile platform. 



7 



Wheel assembly 
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Figure 3.1: A transparent view of Shepherd from above. Showing the “relative” location of the 
CPU, and I/O boards, servoamplifiers, batteries, and wheel assembly. The relative position of the 
vehicle components is subject to change as the vehicle is modified. 



1. Motors and Encoders 

The eight motors and their corresponding shaft encoders used in Shepherd are 
from Yamayo Electric, Inc. These motors allow Shepherd to reach a “theorectical” 
maximum driving speed of 4 kilometers per hour (km/h), and a rate of 1 revolution per 
second about the steering axis (Ref. 7). Figure 3.2 provides the characteristics for the 
driving and steering motors. 
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Servomotor Characteristics 




Driving Motor 


Steering Motor 


Nominal Torque 


1.274 N-m 


0.32 N-m 


Maximum 

Torque 


3.84 N-m 


0.98 N-m 


Nominal 
Rotation Rate 


3000 rpm 


3000 rpm 


Maximum 
Rotation Rate 


4500 rpm 


4500 rpm 


Size 


60 X 123.5 mm 


54 X 86 mm 


Weight 


1.7 Kg 


0.74 Kg 


Power (AC) 


400 W 


100 W 



Figure 3.2: Servomotor characteristics for Shepherd. 

Figure 3.3 illustrates the relative motor (M1-M7) position on the vehicle and the 
general orientation of the vehicle (e.g., front). 



Shepherd Rotary Vehicle 
Motor diagram 




Figure 3.3: Shepherd motor diagram 
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2 . 



Servomotor Controllers 



The servomotor controllers actually provide the commanded voltage and current 
to the driving and steering motors to effect motion. The importance of the servomotor 
controllers can not be understated; the values written to the digital output board (and read 
from the digital input board) are within the acceptable range of the controllers. Also, the 
interface specification matches the range for the motors used on Shepherd and input 
signal voltage corresponds to the range allowed for the driving and steering motors. Later 
sections of this document will show that the voltage produce by the batteries is 
approximately 144 volts, which is within the acceptable range of the controllers. Figure 
3.4 contains the characteristic and interface data for the servomotor controllers. 



Servo Motor Controller Specifications 


Motor Capacity 


400 W 


Interface Specification 


3000 rpm/5000 rpm 


Output Current 


8A 


Control Method 


PWM 


Input Control Voltage 


DC+ 120-150 V 


Input Signal Voltage 


DC+/- lOV 


Input/Output Signal 


8 bit 



Figure 3.4: Servomotor controller’s characteristic and interface data. 



3. Gears 

Shepherd’s reduction gear system contains flat gears, planetary gears, and bevel 
gears. This gear configuration has a 1 :50 gear ratio for both driving and steering [Ref. 8]. 
Figure 3.5 provides a transparent view of the flat gears in the wheel assembly. Due to the 
gear configuration, when the wheels are used for steered then some driving is also 
initiated. The aforementioned driving is cancelled by applying the required amount of 
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“opposite” driving. And this “opposite” driving is based on the 1:50 gear ratio and is 
handled in the Shepherd code (Appendix J .Consolidated header files, line 360). 

Figure 3.6 is a side cut away of the wheel assembly. This cut away shows the 
gears involved in transferring force from the motors to the wheels. Gear ratio and force 
calculations can be obtained from [Ref. 8]. Also, mounted one of the flat gears is a “hall” 
sensor, which Shepherd uses to determine if wheel is aligned. 




Figure 3.5: Flat gears in the wheel assembly (transparent view from the top) 
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Figure 3.6: Side cut away of the motors and gears used in the Shepherd driving and 
steering mechanism (diagram not to scale). 

4. Wheels and Tires 

Shepherd’s wheel diameter is 400 millimeters (mm), and the suspension travel 
range is 100 mm. Shepherd has somewhat ruggedized tires, with a tire friction factor of 
.5. The maximim tire pressure has been calculated as 49.8 pounds per square inch (psi); 
where 36 psi is currently used. 
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5. 



Power Supply System 



Shepherd’s power supply consists of twelve (12-volt) batteries connected serially. 
The voltage generated from the batteries is between 144-150 volts (within the servomotor 
specifications). The batteries have been used for periods up to two (2) hours without any 
noticeable degradation of performance. Figure 3.7 shows the switches required for the 
operational settings of charge, run (battery), run (external) and run (extemal/charge). The 
following are valid switch settings for Shepherd. 

• Charge: 1 -SW-C OFF 

2 -SW-B ON 

3 -SW-A ON 

• Run (battery): 1 -SW-A OFF 

2 -SW-C ON 

3 -SW-B ON 

• Run (external): 1 -SW-B OFF 

2 -SW-C ON 

3 -SW-A ON 



• Run (extemal/charge): 1 -SW-A ON 

2 -SW-B ON 

3 -SW-C ON 



Shepherd Rotary Vehicle 
Power Supply 




Figure 3.7; Shepherd power supply switch diagram 
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Figure 3.8: Simplifled schematic diagram of Shepherd power supply. Showing both an external 
AC source and the 12>volt batteries serially connected. 

An AC source (1 15- Volts) can be used to run Shepherd or to charge the batteries 
(accomplished by the AC/DC converter). Figure 3.8 is a global schematic of the 
Shepherd power supply [Ref. 9]. 



C. SHEPHERD ON-BOARD COMPUTER SYSTEM 

The Shepherd vehicle’s system design is illustrated by Figures 3.9, and is broken 
down into the hardware and software components both of which will be explained in 
greater detail in later. The hardware system is a combination of the mobile platform, an 
on-board computer system, servo drivers, batteries, and a laptop computer for a real-time 
I/O device. The computer system consists of a Taurus board housing two Motorola 
CPUs which are 68040 and 68030, a digital to analog board, a digital input board, a 
digital output board, a digital counter board; and a Versa Module European bus 
(VMEbus) based on Motorola architecture. Servo-controllers are connected to these I/O 
boards. Motor encoders are connedcted to the counter board. 
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Figure 3.9: Diagram of the Shepherd on-board computer system. 
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1 . 



Taurus Board 



The Taurus is a dual-processor, and dual bus architecture, VME single slot, single 
board computer [Ref. 11]. The primary computing engine is a Motorola 68040 processor 
running at 25 MHz. The second processor on Taurus is the Motorola 68030. Although, 
taurus supports several real-time operating systems, an in-house operating kernel, SRK, 
was developed (chapter IV). Taurus also takes advantage of the direct memory access 
(DMA) functions provided by Ethernet, SCSI, and Intelligent Serial Controllers to DMA 
into main direct access memory (DRAM) through an isolation gateway between the 
M68040 bus and the M68030 bus. Moreover, Taurus acts as a fully functional VMEbus 
controller and may operate in Slot 1 of the VMEbus back plane (this is the case for 
Shepherd). Hence, the Taurus board is a powerful VMEbus engine and supports the 
requirements for a real-time operating system and a completely self-contained computing 
environment. Taurus features: 

• 25 MHz M68040 Processor 

• Burst Transfers 

• Ethernet and SCSI with on chip DMA 

• 1 6 Megabytes of DRAM main Memory 

• 4 Megabytes of EPROM 

• 1 Megabyte of Flash EPROM 

• 25 MHz M68030 Processor 

• 6 Serial Ports: 4 (RS-232-D Intelligent Ports with DMA), and 2 using a 
68c681 device 

• 32 Parallel I/O 

• 1 1 , 1 6 bit Timers (cascadeable into combinations of up to 80 bits) 

• Interprocessor Mailbox 

• Dynamic Bus Sizing 

• Real-time clock with battery back up 

• Watchdog Timer and 8 KB of battery back-up Static RAM 
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Why was the Taurus board chosen for this project? In addition to the 
aforementioned characteristics, the board uses the M68040 processor. In previous 
development of the Yamabico-11 [Ref. 12] robot, the M68020 was used. Motorola 
claims that its M68000 series chips are backward compatible. This research has 
concluded that this is mostly true, however on some key issues (math functions and 
assembly code) this has not proven to be the case. These issues will be revisited in 
chapter IV. Again a key asset of the Taurus board is how elegantly and logically internal 
and external interrupts are handled (Figure 3.10). 



VMEbus Interrupts 

o 



Interrupts from 
On>8oard Devices 
-SCSI 




ISM 

Interrupt Steering Mechanism 




VIC068A^IC64 


lRQ-6 


•Ethernet 


i=> 














i=> 

IRQ-x 


M68030 















Figure 3.10: Interrupt handling diagram for the Taurus board [Ref. 11] 

The Taurus board's communications facility adds flexibility to the implementation 
of RS-232 or Ethernet. The M68040 chip is a very versatile processor and powerful 
processor, and can perform 14 different operations at a given time. That is 6 operations 
by the Integer Unit (lU), 3 operations by the Floating-Point Unit (FPU), 4 by the Memory 
Management Unit (MMU), and a Bus interface operation. The number of timers on the 
Taums board is also of a great benefit, moreover Timer 5 will be discussed in chapter IV. 
The M68040 is highly parallel, with a 6-stage integer pipeline, that when filled, will 
execute an instruction for every clock cycle. Moreover, each of the MMUs can 
accomplish a cache access and address translation concurrently. 
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2 . 



Digital To Analog Board 



The Acromag Series 9210 Analog Output Board (AVME9210) provides the 
means for connecting and driving analog circuits with outputs from the VMEbus for the 
Shepherd system [Ref. 13]. The board has 8 channels; each channel has a 12-bit 
resolution. A DAC per channel is used for signal accuracy. The DACs are set up to 
accept either straight binary or two’s compliment data. The board has five programmable 
ranges for output voltages; however, 4-/- 10-Volts will be used with shepherd because it is 
a direct mapping to the maximums for the servomotors inputs. Characteristics of the 
AVME9210 are as follows: 

• 12 bit output resolution 

• individual DAC per channel 

• 8 channels per board 

• Byte or Word data transfers 

• Power up reset 

• Pass/Fail status indicators on the front panel 



DAC Data Register 


Data Register 


Base Address Offset 


Channel 0 
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+8AH 
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+8CH 


Channel 6 


-I-8EH 


Channel 7 


+90H 



Figure 3.11; DAC data register, 8 channels of output. Each channel is a digital to analog converter. 
A two byte address is reserved for each data register. 

A single channel represents a driving or steering motor in Shepherd [Appendix E, 
Motor.c, line 228]. The status control register controls the pass/fail light. The Shepherd 
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code toggles the pass fail light in some instances to ensure the system and code are 
functioning properly. There is a memory location for the board status indicator flags and 
reset. This memory location is one byte in length and is located at base address +81H. 
During the early manual testing this status control register was used exclusively to 
accomplish resetting the AVME9210. Also, each of the aforementioned two byte DAC 
data registers (Figure 3.12) are set up as follows; 



MSB 



LSB 



Dll 



DIO 



D9 



D8 



D7 



D6 



D5 



D4 



D3 



D2 



D1 



DO 



12 bits 

Bit DAC Data 



X I X lx lx 

4 bits 

Undefined 



Figure 3.12: DAC data register (16 bit). Most significant bit (MSB) and least significant bit (LSB). 
The 12 bits are a direct mapping of input values to the board, integer range [1023, -1024]. 



The AVME9210 is located at base address 0xffff0400 and is represented in the 
Shepherd code by the “label” VME9210. 
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Figure 3.13: AVME9210 address to “physical” motor mapping. 



19 



3. 



Digital Input Board 



The Acromag Series 942 1 Isolated Digital Input Board (DIB) provides the means 
for connecting the Digital DC inputs the VMEbus for the Shepherd system [Ref. 14], 
The DIB board isolates all digital inputs from the VMEbus for up to 250V AC, or 350V 
DC on a continuous basis (falls within the constraints of the Shepherd servomotors). The 
pass/fail light on this board is similar to the one used in the digital to analog board 
previously mentioned. And the DEB also has the input channel on light as well. The 
board has 64, 1 bit channels configure as four, 16 bit words. The inputs can be bi-polar 
(with polarity being +/- or -/+ at either end of the channel). The bi-directional polarity 
allows Shepherd to use this for changing the direction of wheel driving or the direction of 
steering with a change of input polarity. The DIB has the base address of OxffffOOOO and 
the “label” VME9421 is used in the code [Appendix J, Consolidated header files, line 
386]. 



4. Digital Output Board 

The Microsystems International Corporation 32-bit Optically Coupled Digital 
Output Board (VMIVME-2170A) consists of VMEbus compatibility logic, data output 
control logic, four 8-bit output registers, and 32 bits of isolated outputs. The VMEbus 
logic contains address decoding logic and data transfer control logic, which provides for 
8- or 16- bit data transfers in the “short” I/O address space. The data output control logic 
selects byte or word transfers to the 32 optically isolated channels. The Shepherd 
research group spent many hours attempting to master this logic — a key problem was 
how to determine where the least significant value was for each data register. However, 
Thorsten Leonardy’s efforts paved the way for a consistent and logical method of writing 
to the aforementioned registers. From this the Shepherd group was able to selectively 
choose combinations of motors for steering and driving [Appendix J, Consolidated 
header files, lines 181-192]. Figure 3.14 shows the register bit definitions [ Ref. 15]. 
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Figure 3.14: Register Bit Definitions 



The output board has the base address of OxffffffOO, which is represented in the 
code by the “label” VME2170. 
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Figure 3.15: VME2170 bit assignment. Note 3 bit assignment for driving or steering motor selection. 
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Figure 3.15 indicates that “masks” could be written to the VME2170 base address 
and used for motor selection. Hence, writing a mask for 0x00000004 would select, motor 
1 (Ml) in wheel 1 for driving. And writing a mask for 0x00004000 would select, motor 
5 (M5) in wheel 1 for steering. Moreover, using a mask for 0x00924924 all motors can 
be selected (using a “logical And” on the true values for each motor selected). 



5. Counter Board 

Shepherd uses the Green Spring IP-Quadrature Four Channel Quadrature Decoder 
or counter board [Ref. 16]. The counter board reads the signal produced by the encoders 
(see section III.B.l of this document); this signal is “index” pulse once per revolution to 
provide absolute position information. There are four channels on the counter board; 
each channel has three inputs. The inputs are normally called X, Y, Z, and the board 
inputs are X and Y. Z is the control or index input. Each channel has a 24-bit up/down 
counter block, 24-bit capture/match register and a 24-bit output latch allow for an 
accurate “on the fly” reading of Quadrature position values. The up/down count direction 
in the counter board is controlled by the relative phase of the X and Y inputs. Count 
direction can be reversed by: reversing the mechanical motion direction, reverse the 
connections for X and Y, reverse the X polarity bit, or reverse the Y polarity bit. 
Shepherd uses a function called “readEncoders” which is an excellent example of 
completing three consecutive 8-bit reads from the counter board and catenation the 8-bit 
segments into a single 24-bit position reading [Appendix E:, Motor.c, lines 300-356 and 
606-620]. The counter board’s base address address is 0xffff6000, and is represented by 
the label VMECTRl in the Shepherd code. 
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IV. SHEPHERD SOFTWARE SYSTEM 



A. OVERVIEW 

The Shepherd software system consists of the software development environment 
(including GCC version 2.7.2. 1 compiler), Shepherd Real-time Kernel (SRK), and the 
firmware on the Taurus board. Shepherd software is developed in the “C” language on a 
Unix workstation, and the code is cross-compiled using switches with the GCC compiler 
to ensure viability on the Taurus board (step by step instructions are in the Shepherd 
Operating Manual, Appendix K). The code is in “S” record format and is transferred (via 
Ethernet) to a laptop computer, which is used as the user interface. When the user is 
ready to test or run a program, it is then download via RS232 to the RAM on the Taurus 
board using the Taurus bug or firm-ware on the Taurus board. The SRK has real-time 
timer control. The timer interrupts are set for 10 milliseconds, but can be modified to 
suite user needs and requirements. SRK’s central motion control sections are shepherd.c, 
user.c, and the driver routine (all of which will be discussed in great detail later). The 
user interface viewed from the laptop computer is also generated from the code in user.c 
and other I/O code segments. The overall software environment and firmware works 
together to form a tightly coupled and low overhead “operating kernel” that is the SRK. 
The use of the SRK allows the user to control (i.e., timer interrupt, motion control, and 
user interface) the Shepherd rotary vehicle. 



B. SOFTWARE ENVIRONMENT 

The software environment (minus the SRK) includes the following: Bug monitior, 
S-records, and Software development environment. 

1. Bug Monitor 

The “taurus bug” is a powerful debugging and evaluation tool, and is firmware on 
the Taurus board [Ref. 20]. It has facilities for loading and executing user programs 
under complete operator control and evaluation (and is used extensively in Shepherd 
development). The “taurus bug” includes commands that allow the user to display 
memory, modify memory, set and remove break points, an assembler/disassembler, and a 
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system self test capability that verifies system integrity upon power up. The “taurus bug” 
also has various routines to handle some I/O, string functions, and data conversion via the 
TRAP #15 handler on the Taurus board (used in Appendix C, lines 535-548). Moreover, 
on power up, all static variables are set to default states, the break point table is cleared, 
all target registers invalidated, I/O character queues cleaned, the vector interrupt table is 
written to RAM, and all on-board devices (serial ports, timers, etc.) are cleared or reset. 
Taurus bug also has a system reset and abort feature. The system reset completely re- 
initializes the board and the abort feature captures a snapshot of the processors present 
state — allowing the use of stack pointers, and the program counter to help determine 
errors (the hardest way to debug). The “taurus bug” was a very valuable tool; however, 
at times it was difficult to use. And tracing through assembly code to resolve a problem 
using the “Trace” function and the symbol table can take numerous man-hours -without 
immediately yielding a positive result. A very important function of “taurus bug” is the 
loading capability. The use of the “Lo” command to place executable code in memory is 
key in the development process. The “S” records are downloaded to the Taurus board in 
this manner. 

2. “S” Records 

The S-record format was devised by Motorola for output modules. Its key 
purpose was encoding programs or data files in a printable format for the transportation 
between computer systems. Hence, providing a way of visiually monitoring the 
transportation processs and a method of quicky editing the code if required. S-records 
are character strings made of several fields which identify the record type, record length, 
memoery address, code data and checksum (see Figure 4. 1 ). 



Field 


Printable Characters 


Contents 


Type 
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Character pairs in record 
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Address 
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The 2, 3, or 4 byte address 
at which the data is loaded 
into memory 
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Field 


Printable Characters 


Contents 


Code/Data 


0-2n 


From zero to 2n bytes of 
executable code, 
descriptive information, 
or loadable data. 


Checksum 


2 


Least significant byte of 
I’s complement of the sum 
of the values represented 
by the pairs of characters 
making up records length, 
address and the code/data 
fields. Used for error 
checking 



Figure 4.1: S-record content chart [Ref. 20]. 



S-records module may contain the following types (and many more): SO (header 
data), record containing address where code is to reside in memory (SI, SI, or S3), S5 
(the number of records transmitted per block), and the termination record (S7, S8, or S9). 
A typical S-record might look like this: 

S00600004844521B 

SI 1 3000284F245F2212226A000424290008237C21 
S 1 1 300100002000800082629001 853812341001813 
S9030000FC 

A detailed byte wise explanation of S-records is contained in Ref. 20. The S-records for 
shepherd’s development are generated during the linking and loading process. A special 
switch is used that allows the creation of a file named “shepherd.TXT”. The 
“shepherd.TXT” file contains the S-records to be downloaded. 
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3. 



Software Development System 



As mentioned in this section's overview the compiler used is the GCC 2.7.2. 1. 
This compiler posed many problems for SRK development. One of the biggest obstacles 
was the passing of “composite” structures. Structure values had to be placed into dummy 
variables (Appendix D, lines 138-140) in order to get the code to execute. This was 
especially odd because the code compiles, but will not execute (if the dummy variables 
are not used). Also, several of the compiler switches that supposedly allow mathematical 
code (that was previously valid for the M68020 with math co-processor) would simply 
not compile. An inordinate amount of time was spent trying to resolve this issue because 
one of the initial precepts of the project was that old code from the Yamabico-1 1 robot 
system would be portable. There are still anomalies that work arounds were developed 
for. For instance, there is a “square root” function that compiles and works in every 
environment (other compilers) yet would never function when compiled using GCC 
2.7.2. 1. Moreover, because the compiler does not have libraries for I/O or standard math 
functions, these had to be derived to support the M68040 and Taurus board (again taking 
an inordinate amount time). The compiler did serve its purpose — because its freeware 
(the budget did not allow for commercial compilers). 

One switch that did work was -m68040, which allowed the generation of M68040 
specific code. The "makefile” makes use of the -m68040 (segment below): 

shepherd.o : shepherd.c 

gcc -c -m68040 -o shepherd.o shepherd.c 

timer.o : timer.c 

gcc -c -m68040 -o timer.o timer.c 

user.o : user.c 

gcc -c -m68040 -o user.o user.c 

motor.o : motor.c 

gcc -c -m68040 -o motor.o motor.c 
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The early testing also required a complete understanding of the “a.out” generated 
from the assembler and the link editor (the link editor makes “a.out” executable files). 
The “a.out” (Figure 4.2) consists of: a header, program text, program data, text and data 
relocation information, a symbol table, and a string table (the last three sections may be 
omitted if the program is loaded with the -s option. All of the aforementioned 
information was useful because our earliest a.outs were in the wrong format, hence not 
useable by the M68040 due to using non-functioning compiler switches. 



A.OUT 




Header data 



Figure 4.2: A generic A.OUT format. Note many features of the A.OUT 
are left off, such as symbol table, entry point, dynamic, and machine type. 

The final element to be discussed is how the link editor is used. First, we must 
discuss the DRAM Memory map (Figure 4.3). The Taurus board documentation [Ref. 
21 ] warns that accessing the memory below $10000 (Hex), hence the memory the format 
used in Figure 4.3 with 16 Megabytes as the upper bound. 
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DRAM Memory Map 



$100000 

or 

16 MB 






END DRAM 



Region Available for 
User program 



$10000 

$0 



T 

Region used by 
“taurus bug” 



Start user DRAM 



Start DRAM 



Figure 4.3: DRAM memory map. It should be noted that to be able to write to 
the DRAM the Parity ISM register must be disabled or the block- fill command 
(taurus bug) used to clear the region required [Ref . 12, and 22]. A detailed 
example is in the Shepherd Operating Manual (Appendix K). 

Again the Shepherd makefile is illustrative of the linking process: 

comp: startup.© shepherd.o timer.o serial.o math.o udls.o utils030.o user.o\ 
motor.o movemento 

Id -Ttext 0x10000 -Tdata 0x20000 -Tbss 0x30000 -Map shepherd.map 
o format srec\ 

-o shepherd.TXT startup.© shepherd.o dmer.o serial.o math.o udls.o\ 
udls030.o user.o motor.o movemento 

The make file shows the text segment of the code being loaded at 0x10000, the data 
segment of the code loaded at 0x20000, and the upper bound for the data at 0x30000. 
Hence, the code is loaded within the parameters required by the memory map. Earlier it 
was mentioned that the S-records were generate by the linker — the “oformat srec -o 
shepherd.TXT” generates the required S-records for download to the laptop PC. If the 
“oformat srec” switch is not used then the standard a.out will be generated. The -Map 
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switch also allows the user to generate a symbol table (called shepherd.map here) for use 
in debugging. At this point all the underlying structure for SRK development is in place. 

C. SHEPHERD REAL-TIME KERNEL (SRK) ARCHITECTURE 

The SRK includes the Timer control, Motion control, and the User interface. 
Figure 4.4 below illustrates the exact architecture of the motion control part of the 
system. 




Figure 4.4; Shepherd Motion Control Architecture 



1. Timer Control 

The timer control is a very integral part of the system. As was stated earlier SRK 
has a real-time modifiable timer control. This timer control is made possible through the 
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use of the Taurus board’s AM9513A Counter/Timer. This Timer/Counter provides five 
16-Bit general purpose counters and uses a 4-Mhz oscillator as a clock input with the 
outputs connected to the Taurus Interrupt structure for processing. The Timer 5 group of 
the AM9513 device was utilized due to it’s compatibility with the 68040 Processor and 
contains it’s own timer handler. 

The main routine in controlling the timing and setting the interrupts is located in 
the timer.c file along with the header file in timer.h (Appendix F). This timer is 
initialized and started in the main shepherd routine. It continuously provides a 10 
millisecond interrupt until the program is terminated. However, this value could be 
modified. This was made possible by manipulating the data port values of the AM9513-1 
device and multiplying those values by factors of 10000, 1000, 100, 10, or 1 to obtain a 1, 
0.1, 0.01, 0.001, or 0.0001 second interrupt in that order. The accuracy of this timing 
was tested using an oscilloscope and also a frequency analyzer. It was found through 
testing that the SRK when in operation only utilizes approximately 2.7 milliseconds total 
in handling all the associated routines. For further information on this counter/timer see 
reference 21. 



2. Motion Control 

Referring back to Figure 4.4, motion control encompasses several major parts 
which are woven into a tightly controlled structure for driving and Steering Shepherd’s 
wheels. The bodyMotion function takes as input the mode of motion desired from the 
user. Using this mode and the necessary instructions programmed, it provides 5 inputs to 
the wheelMotion function which are: motion, speedDot, thetaDot, omegaDot, and 

vehicle. Motion is a structure consisting of the user’s input of speed, theta (direction of 
travel of the vehicle), and omega (rotational speed of the vehicle). Vehicle is a structure 
consisting of the x and y coordinate of the vehicle on some x-y plane for tracking 
purposes and the heading of the vehicle. SpeedDot, thetaDot, and omegaDot all are a 
derivation of speed, theta, and omega over time. 

The wheelMotion function takes the given inputs along with inputs from a 
feedback loop for the actual positions of the encoders for both driving and steering of the 
vehicle, and performs calculations based on the theory discussed in chapter VI. The 
results of these calculations are then sent to the driveMotors function which provide these 
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values to the servo drivers for steering and driving the vehicle wheels resulting in the 
motion of Shepherd. 



3. User Interface 

The user interface is facilitated through the use of a Texas Instruments laptop 
computer running the Windows 95 operating system. By utilizing the HyperTerminal 
program accessory to connect the laptop to the unix system via an ethemet connection, 
program files can easily be receive from the unix system after being compiled and the 
executable files sent to the taurus board. After successful downloading of the program to 
the Taurus board, the user can then control the operation of Shepherd through the laptop 
keyboard. Figure 4.5 illustrates this user interface after the program is successfully 
downloaded and ran. An on-screen menu is displayed with a description of Shepherd 
motion that can be initiated via keyboard strokes. 

This menu is located in the file user.c (Appendix C). It is a simple character 
definition which is called from the user.c file. The menu choice inputs from the keyboard 
are converted to ascii characters which are interpreted in the user() routine switch 
statement to launch the associated menu item seen in figure 4.5 on the next page. 

Also, part of the user interface is a joystick. This joystick interface is activated 
when menu choices 3 and 4 are selected. It controls Shepherd’s wheels for both steering 
and driving. This will be discussed in Chapter VI. 
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Figure 4.5: Display of shepherd menu for user interface. 
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V. EXPERIMENTAL RESULTS ON MOTION CONTROL 



A. OVERVIEW 

Chapters III and IV of this thesis discuss how each servomotor can be accessed 
and voltages applied. However, the aforementioned chapters do not explain how the 
incremental inputs placed on the DA translate to wheel speed or what the maximum and 
minimum wheel speeds are. So, how were the maximum and minimum wheel speeds 
determined? Moreover, what type “controls” are required to ensure that each wheel has 
the same driving velocity or angular velocity? We used a modified version of the 
“scientific method” to establish and carry out the experiments for the Shepherd vehicle. 

Scientific Method and Approach 




Figure 5.1: Modified version of the scientific method and approach used for Shepherd development. 
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As with any experiment, results must be consistent and reproducible. Considering 
the vehicle as a “closed system” observation of all experimental results should lead to 
deduction, connection, or correlation. As is common with the scientific method progress 
can be very rapid or slow. Sometimes there were unexpected results (e.g., singularities), 
and in some cases our dead-end path (failed experiment segment) provided some very 
important insights which helped to improve vehicle performance. An Ockham’s razor 
approach was taken when making investigations and deductions — meaning that any 
unknown phenomena or behavior should be explained in terms of what is already known 
(and testing the simplest possibility first). 



B. WHEEL DRIVING 

Once an understanding of the mechanics and providing the coded structures to 
move the wheels had been achieved the concept of precise and controlled driving motion 
becomes the focus. The basic initial idea was to measure the counts from the counter 
board, measure wheel revolutions over time. Again reiterating a modified scientific 
method was used — and apriori data such as gear ratio and other engineering data were 
used to verify results. 



1. Developing Driving Constants 

The digitToRadDrive constant [Appendix J, Consolidated header files, line 364] 
was the first constant to be determined for wheel driving. The biggest problem with 
developing this constant was working around the 10-millisecond timer interrupt and the 
lack of a fully functional operating system. Since Shepherd’s SRK was developed from 
the ground up within the last year it has limited I/O capabilities, and there is no long-term 
storage on the Taurus board. What this means is that the results of test programs would 
have to be sent to a monitor (VT220) or to a printer. We decided to print to both the 
monitor and printer. However, this also yielded unexpected results. The print function 
used too much of the 10 milliseconds to allow for proper functioning of the process 
currently running. Hence, we moved the print function from the real-time portion of the 
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code to alleviate this problem. Moreover, this became a useful technique that was used 
throughout Shepherd development and testing. 

The first step in determining the number of counts is reading the counter board. 
In the algorithm used in the SRK, the initial values on the counter board are read, stored, 
and read again after a complete revolution of the tired is complete; the absolute difference 
between the initial counter value and the final counter is the actual count take for the 
revolution. It should noted however that consecutive reads of the counters is actually 
accomplished and the values adjusted for the transition from Oxffffff to 0x000000 and 
vice versa (24 bits) [Appendix E, Motor.c, lines 607-621]. An example of the code to 
read the encoders for driving can be seen below: 

void readDriveEncoders (unsigned long int arrayQ) 

{ 

unsigned char *p= (unsigned char*)VMECTRl, cl, c2, c3; 

int ix; 

long int temp; 

for (ix=0; ix<4; ixH- +) { /* read all four motors subsequentially */ 

*(p+3)=0x03; /* load output latch from counter */ 

*(p+3)=0x01; /* control register, initialize two-bit 

output latch */ 

/* read three bytes for specific counter ix and save in status */ 

/* first access to Output Latch Register reads least significant */ 

/* byte first */ 

cl =*(p+l)&0x00ff; 
c2 = *(p+l) &0x00ff; 
c3 = *(p+l)&0x00ff; 

array[ix] = ((unsigned int)cl) | ((unsigned int)c2 << 8) | 

((unsigned int)c3 << 16); 

p=p-H4; /* increment pointer for next counter */ 

} 

return; 
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} /* end of readDriveEncoders */ 



Secondly, a method to display the enconder data is required. The SioOut 
[Appendix E:, Motor.c, lines 483-490 and 381-399] display routine from SRK was used 
for this, but failed because the time required to display the data exceeded 10 milliseconds. 
So, we moved the display “call” for the function from the real-time portion of the code. 
In SRK, immediately following the “driver” routine’s call the following type routine 
would be placed in the main “user” routine: 



The aforementioned routine would print based on the edCounter and the timer. The 
counter being incremented every 10 milliseconds. Hence the value is displayed every 2 
seconds in real-time. The key is the fact that the printing is being executed outside of the 
driver routine. 

Now all the elements are in place to determine the digitToRadDrive constant. 
After testing each wheel for a minimum of 1000 revolutions (called double pi or DPI 
below) the digitToRadDrive constant was determined: 

#define digitToRadDrive -6.015495746e-5 



while(l) 



{ 

while (edCounter%200 != 0){}; 




/* driving constant rad/count = DPI/104450 May 8 */ 
/* Experimental Results by Ed Mays May 7 */ 



/* Wheel 1 count = 104456 
/* Wheel 2 count = 104435 
/* Wheel 3 count = 104454 
/* Wheel 4 count = 104455 
/* Average count = 104450 
/* cf. 2048* 51 = 104448 



V 

V 

V 

V 

V 

V 
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Verification of the constant was made possible by the engineering data. Given the range 
of inputs (same as Figure 3.12) to the Servomotors and the gear ratio (section 3.B.3) the 
digitToRadDrive was be verified above (cf. 2048 * 51 = 104448). The value of 51 vice 
50 for the gear ratio (was determined empirically to be 1:51). 

The digitToCmDrive was much more trivial to determine. The digitToRadDrive 
provides the number of counts, knowing the wheel radius (18.9cm), and using circle 
circumference formula yields: 

#define digitToCmDrive 0.0011369287 

/* driving constant cm/count = digitToRadDrive*! 8.9cm Ed Mays */ 

/* 5/8/97 */ 

Now having the “drive” encoder count allows the computation of distance traveled (cm) 
by the vehicle; and coupled with the timer interrupt allows for the computation of 
velocities and accelerations. 

2. Measuring Wheel Speeds 

The next step in control of the wheels is being able to manipulate wheel speed. 
Developing wheel speed control was one of the projects many dead-end path’s — that 
eventually lead to great results. There were three problems with the work presented here. 
First, the inputs or digits were not applied at the lowest (hardware) level using 
SpeedDigits [Appendix E], instead the value was considered desiredSpeeds. Secondly, 
the input values were massaged before being used to ensure that all the system hardware 
would accept the range of values. Thirdly, the uniqueness of each driving motor and 
natural output variance over a range of inputs was not considered (e.g., used averages 
instead of individual motor data). How did this happen? A minor communication error 
and poor naming of arrays made this possible. However, a look at the results and the 
logic for deriving them will provide insight towards the actual solution. 

Include are an estimated velocity and the software-measured (actual) velocity as 
determined by software. The estimated velocity was determined by applying the 
requisite input and measuring the number of seconds it took for wheel one to complete 10 
revolutions (e.g., wheel radius 18.9cm, distance traveled = 2 * PI * Revolutions * radius 
= 1187.5220 cm); inaccuracies from this measurement came from hand timing and the 
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version of pi used on a desk calculator. Velocity calculated from V=distance (cm)/time 
(seconds). 

The software-measured velocity used the digitToCmDrive constant, which has the 
unit’s cm/count. The algorithm subtracts the previous count from the present counter (as 
read from the counter board) and multiplies the result by digitToCmDrive leaving the 
outcome with the unit of cm. This outcome is then divided by .01 (DeltaT) representing 
100th of a second or 10 milliseconds (corresponding to the system interrupt). The 
99999999 represents a value not representable by the counter board. The output 
displayed to the monitor every 100 calls of the routine (mod 100). An example of the 
code to compute each wheel’s driving speed can be seen below; 

void computeActualRatesQ 

{ 

int i; 

double count, speed; 



for(i=0; i<=3; i++) 

{ 

if(PreviousCountSpeed[i] == 99999999) /* for derivative for speed */ 
Drive__Speed_Actual[i]= 0.0; 
else 

Drive_Speed_Actual [i] = 

(convertDifference((WheelDriveValues[i] - PreviousCountSpeed[i])) 
*DigitToCmDrive[i])/DeltaT; 

PreviousCountSpeed[i] = WheelDriveValues[i]; 

} 
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Input 

(digits) 


Time 

Stop watch 
(sec) 


Estimated Velocity 
(cm/s) 


Software 

Measured Velocity 
(cm/s, average) 


10 


116.27 sec 


10.21349 


10.23235 


20 


58.15 


20.42170 


20.46.471 


30 


39.10 


30.37141 


30.35599 


50 


No data 


No data 


50.70702 


60 


No data 


No data 


60.93937 


70 


No data 


No data 


70.94435 


80 


No data 


No data 


81.29040 


90 


No data 


No data 


87.65720 



Figure 5.2: Inputs and results from massaged data (error). No data entries exist because the 

revolutions were too fast for hand timing. 



If an input of 100 is used the estimated values for velocity are nolonger linear. If the 
inputs vs. velocities were linear the ratio 20/20.421 70= 100/X used to predict the 
estimated velocity would yield a velocity of 102.32355 cm/s; however, the software- 
measured velocity was 87.54350 on average for all input values from 100 to 1000 (Figure 
5.3). 



Input 

(digits) 


Time 

Stop watch 
(sec) 


Estimated Velocity 
(cm/s) 


Software 

Measured Velocity 
(cm/s) 


100 


No data 


No data 


87.65720 


200 


No data 


No data 


87.65720 


400 


No data 


No data 


87.65720 


1000 


No data 


No data 


87.65720 



Figure 5.3: Inputs vs. measured velocity. Figure showing a velocity saturation. 



Hence, the relationship between the input and the velocity look somewhat linear until the 
area where the input is greater than 70— after this velocity saturation seems to occur (or 
maximums reached). Moreover, if one closely looks at the slope (i.e., y = mx + b) 
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between the input ranges 20-30, 83-84, and 87-88 it is clear that the slope changes 
dramatically over these regions. Even though the wrong input structure and mechanism 
was used to generate this data a lot was learned. For instance, motor performance is not 
completely linear and each motor has somewhat unique characteristics. Once the 
aforementioned problems were identified and corrected the correct data [Appendix N] 
could be plotted. Figure 5.4 is a plot of the correct data. 

Driving Velocity vs. Input 



saturation 




Figure 5.4: Driving Input vs. Velocity plot. Input units or digits arc written directly to SpeedDigits. 
The slope changes for the overall graph are constant, making it look linear. However, over some 
regions this graph is non-linear, this will be dealt with under the controls section. Velocities also 
independently verified by use of tachometer. 



3. System Controls 

So far controlling the speed of individual servomotors has been discussed. 
However, it is known individual servomotors provide differing outputs for the same input 
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ranges in some cases. Moreover the wheels must be coordinated and work together. The 
goal of this project was to have an actual driving speed that has less than 1 % error when 
compared to the desired input speed (for each wheel). How is this close tolerance 
accomplished? This small margin of error is accomplished by using well-established 
concepts from “control systems” theory. A general control-system structure contains 
inputs (or reference commands), a controller (with external power), control forces, a 
controlled system (plant), disturbance inputs, outputs, and output monitoring. Control 
systems are almost a discipline unto themselves requiring knowledge of differential 
equations and Laplace transforms. Shepherd is looked at as a closed-loop where output 
monitoring is accomplished through sensors (encoders) and the information passed 
through feedback channels. The feedback results in a closed loop signal or information 
flow. The controller design for Shepherd is linear and considered a single-input-single- 
output (SISO) system. Hence, conceivably the state-variable or the transfer-function 
(input-output) method could be used here. What technique did we use? We used the trial 
and error technique [Ref. 17]. The trial and error technique (Figure 5.5) was chosen 
because of the skill and knowledge levels of the Shepherd team. The advantages of this 
technique are: 

1 . Simple mathematical tools are used 

2. Vast amounts of experience have accumulated 

3. Especially well adapted for use with computers 

4. Linear designs usually are acceptable 

The disadvantages are: 

1 . Inconsistent performance specifications (PS) can be encountered 

2. Design is not optimal 

3. The method is usually suitable mostly for SISO systems. 
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Start 
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Select PS 



i f 



Select 

configuration 



I £ 

Set 

parameters 



1 



Select new 
PS 



yes 



No 



Can PS be met 
by selecting 
new 

configuration? 



yes 



No 




Figure 5.5: A flow graph of the triahand-error design process [Ref. 18]. 

Using the experience of both Professors Kanayama and Yun as the guide the 
“black box” servo structure was developed (Figure 5.6). The servo structure is called 
“black box” because of the lack of understanding of the servomotors at that time 
(however, inputs and outputs could be measured). The previously mentioned PS was an 
error rate of less that 1% of the given reference input. There were other PS’s governed 
by heuristics. For instance, it was desired that the actual output velocity converges (in 
the mathematical sense) on the reference or input velocity, with near perfect static follow- 
up. 
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Shaft Encoder 
(posit or^ 




Figure 5.6: “black box” servo structure vv^ith feedforward and feedback compensation. 

Vibrations and other unacceptable behaviors were used to determine if a smooth 
and acceptable convergence had been achieved for the gain used (e.g., if the robot was 
shaking, this was not acceptable). An acceptable gain would be one that produces an 
oscillatory response that converged quickly to the reference input (Figure 5.7). Again 
this same type information could have been determined by a better scientific guess [Ref. 
19] using a closed-loop differential equation. 



Response as a Function of 




Figure 5.7: Proposed “proper” oscillator^' response as a function of loop gain. Response determined 
for Shepherd using experience and heuristics. 
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Previously, it was stated that the input vs. velocity plot was not “truly” linear. 
These non-linear ranges have severe physical realities. Also, mentioned was the point 
that each servomotor produces an output that may not be the exact same as the outputs of 
the other system servomotors given the same input. And the stated PS requires that 
servomotor outputs be within 1% of the desired input or input speed. This translates into 
several challenges. First, using the feedforward technique constants were developed to 
ensure that the PS of 1% is met. The following algorithm or averaging technique was 
used for direct testing of inputs: 

if (V] < V < V2) 

d = di + (d 2 - di)/(v 2 -vi) * (v - vi) 

The v’s and d’s above are the same as the velocities and inputs in figure 5.8. 




Figure 5.8: DIustration of averaging technique used to select “good” 
input ranges. 

Using the aforementioned averaging technique the constants were developed that 
maintained all servomotor outputs within \% PS. 
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These constants are called K1-K4, and K6. These constants represent the 
feedforward values that are applied to each servomotor over a specified range of desired 
speeds. The desired speed ranges give are because of the piecewise continuity chosen 
because of the non-linearity’s in the input vs. velocity plot for the servomotors. The “K” 
constants were developed with no load on the system (e.g., wheels free floating). Listed 
below are the constants K1-K4, and K6 (speed is in centimeters/second): 

Kl[0]=11.448;/*0<=speed<=5*/ 

Kl[l]=l 1.500; 

Kl[2]=l 1.496; 

K1 [3] =12.375; 

I<2[0]=1 1.500;/*5>speed<8*/ 

K2[l]=l 1.500; 

K2[2]=l 1.644; 

K2[3]=l 2.000; 

K3[0]=l 1 .61 l;/*8>=speed<20*/ 

I<3[1]= 11.585; 

K3[2]=l 1.686; 

K3[3]= 11.840; 

K4[0]=l 1 .71 l;/*20>=speed<=70*/ 

K4[l]=l 1.659; 

K4[2]= 11.705; 

K4[3] = 11.727; 

K6[0] = 11.710;/*70>speed<K5*/ 

K6[l]=l 1.700; 

K6 [2] =11.700; 

K6(3] = 11.715; 



Above the bracketed values are array element numbers. Element [0] refers to 
wheel 1, motor Ml; element [1] refers to wheel 2, motor M2; element [2] refers to wheel 
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3, motor M3; and element [3] refers to wheel 4, motor M4. A constant K5 was also 
defined as 87.4 cm/sec, and used a safety cutoff for the maximum speed. Figure 5.9 
shows the values for a few selected desired or commanded speeds (most are within the 
require 1% error in PS). 



Desired Speed 


Ml Speed 


M2 Speed 


M3 Speed 


M4 Speed 


10 


10.20 


10.08 


10.10 


9.89 


20 


20.12 


20.11 


20.11 


19.93 


30 


30.03 


30.00 


30.02 


29.98 


40 


39.98 


39.91 


39.98 


39.98 


50 


49.98 


49.79 


50.08 


50.00 


60 


60.01 


59.77 


60.02 


60.02 


70 


69.91 


69.78 


69.95 


69.97 



Figure 5.9: Desired (commanded) speeds vs. actual ‘‘free floating” motor speed. The application of 
feedback is expected to move the speed of Ml into the required 1 % error for the PS. 



At this point all the tools and techniques (i.e., trial-and-error, scientific method, 
and the experience of Professor’s Kanayama and Yun) are in place for the application of 
system controls. How are the system controls (Figure 5.6) implemented? First, the 
algorithm is presented: 

SpeedDigit = velocityReferenceTable(Omega_Speed,ix) + 

DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual[ix]); 

SpeedDigit represents the actual digit value being applied to the servomotor. In the 
feedforward part of the loop (Figure 5.6) the commanded velocity is multiplied with a 
constant KJ, which corresponds to the "K" constants described earlier in this section? In 
the code segment above the function “velocityReferenceTable” is called (prior to the 
addition of DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual[ix]) ). The 
“velocityReferenceTable” applies the proper “K” constant for the range the commanded 
speed (Omega_Speed) falls within. 
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A detailed look at velocityReferenceTable is provided below: 



double velocityReferenceTable(double desiredVelocity,int i) 

{ 

double inVelocity, 
outVelocity; 

inVelocity=new_abs(desiredVelocity); 

if (inVelocity>=0.0 && inVelocity<=5.0) 
outVelocity = inVelocity*Kl[i]; 

if (in Velocity >5.0 && inVelocity< 8.0) 
outVelocity = inVelocity*K2[i]; 

if (inVelocity>=8.0 && inVelocity<20.0) 
outVelocity = inVelocit)^I<3[i]; 

if (inVelocity>=20.0 && inVelocity<= 70.0) 
outVelocity = inVelocity* K4[i]; 

if (inVelocity>70.0 && inVelocity<K5) 
outVelocity = inVelocity*K6[i]; 

if (inVelocity> K5) 
outVelocity^ 1 023; 

if (desiredVelocity< 0.0) 
outVelocity = - outVelocity; 

return outVelocity; 

} /* end velocityLookupTable */ 
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It should be noted that if the inVelocity is greater than K5, then the outVelocity is given a 
value of 1023 — this ensures there are no system resets because the input digits are too 
large. All other velocities are multiplied by a specific “K" and the value returned. In the 
feedback part of the loop (Figure 5.6) the difference between the commanded velocity 
and the actual velocity is multiplied with a constant Drive FeedBackGain (also K2 in 
Figure 5.6) [Appendix E, lines 46 & 47]. Again this 

(DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual[ix]) ) is added back into the 
inputs used for the next time the process is run (based on the 10 millisecond timer 
interrupt). 

Hence, now the final key to this control system would be finding a 
DriveFeedBackGain constant that would provide the desired “proper” oscillatory 
response (Figure 5.7) and ensure the servomotor output velocity is within the 1 % error of 
the commanded velocity required by the PS. Using Occam’s Razor, the trial-and-error 
flow diagram (Figure 5.5), the iterative approach in the scientific method (Figure 5.1), 
and the heuristics provided by Professor’s Kanayama and Yun, the search for the best 
DriveFeedBackGain was initiated. The heuristics used were observation based. First, 
the “gain” used could not cause the vehicle to shake in any visible manner. Secondly, the 
“gain” used had to quickly move the actual servomotor speed to the commanded speed if 
there was a difference. Pseudo random values were chosen as gains, based on the 
experience of the aforementioned professors. The real number range [-1.0, 1.0] was used 
to test the gains. On the negative end of the range the gain was incremented by +.05, 
until the gain equaled zero — the results were not acceptable. On the positive end of the 
range the gain was decremented by +.05, until the gain equaled zero — at .8 the gain 
showed the best results (i.e., range [0.0, 1.0]). The gain was defined as 

DriveFeedBackGain = .8 [Appendix J, Consolidated header files, line 389]. Hence the 
gain met all the criteria for an acceptable design (Figure 5.5) and validates the “black 
box” servo structure as envision by professors Kanayama and Yun (Figure 5.6). 
Moreover, the experiment can be considered a multidisciplinary success between physics, 
electrical engineering, and computer science. 
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c. 



WHEEL STEERING 



As with wheel driving, the same approach to wheel steering was used in 
measuring the counts from the counter board and measuring the rate of turning of each 
wheel over time. The goal was to observe and measure each wheels turn rate and create a 
feedback compensation loop as in wheel driving to ensure that the PS of 1% is met. 



1. Developing Steering Constants 

The steering constants were developed in the same way as the driving constants. 
The counter board was read for steering values and then displayed . A digitToRadSteer 
(input digits per radians for steering) constant value of -6.8 1769239 le-5 (rad/count = 
(2*pi)/(2048*45)) and RadRateTodigit constant value of 195.4155 (digits/rad/sec = 
1023/5.23598) was determined by observation of the data forthcoming. 



2, Measuring Wheel Rate of Turn 

In measuring the wheel rate of turn, the same approach was taken as discussed 
before. An estimated rate of turn and the software-measured (actual) rate of turn are 
included. The estimated rate was also determined by applying an input and measuring 
the number of seconds it took for a wheel to completely rotate 360 degrees. As can be 
deduced, a certain amount of error was introduced due to human timing interaction. 



Desired rate of 
turn 

(rad/s) 


Time 

Stop watch 
(sec) 


Estimated Rate 
(rad/s) 


Software 
Measured Rate 
(rad/s, average) 


1 


6.0 


1.00000 


0.98174 


2 


3.5 


1.79485 


1.95667 


3 


2.19 


2.86849 


2.93160 


5 


1.69 


2.71716 


3.90653 


5.5 


No data 


No data 


4.88828 
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Desired rate of 
turn 
(rad/s) 


Time 

Stop watch 
(sec) 


Estimated Rate 
(rad/s) 


Software 
Measured Rate 
(rad/s, average) 


10 


No data 


No data 


5.23598 


20 


No data 


No data 


5.23598 


30 


No data 


No data 


5.23598 



Figure 5.10: Inputs and results from massaged data (error). No data entries exist because the 
revolutions were too fast for hand timing. 



By observing the data from the above figure we can confirm that a certain amount 
of error is inherent in the system. As the input value increases from 0 to 5.5 radians per 
second , a linear correspondence tended to exist. However, after an input of 5.5 rad/s the 
software measured average tended to be 5.23598 rad/s resulting in a saturation state. This 
result in itself could not give us the exact range of values where this occurred. Therefore, 
manipulation of the steerDigits was necessary. Appendix O contains these results given 
in inputs of digits vice desired rate of turn. At the maximum input speedDigit of 1023 
saturation is reached. The wheels will only turn at an average rate of 5.235 radians per 
second. Thus the software measured average of 5.23598 rad/s was adopted. 



3. Steering Feedback 

By further observation of the data obtained, an average was used in obtaining the 
rate of turn. Not all the wheels turn at the same rate. The objective was to have a less 
than 1% PS for optimization. To achieve that another “black box” servo structure was 
developed . The Figure 5.11 is a pictorial representation of this and is a little different 
than that of wheel driving. 
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Figure 5.11: “black box” servo structure with series and feedback compensation for wheel steering 
feedback control. 



In finding the K constants from the figure, trial and error is also used. The 
representation of the algorithm in code is presented; 

Steer_Digit = rateReferenceTable(desiredAngleRates[ix]) 

+ steerFeedbackGain*(desiredAngleRates[ix]-actualAngleRates[ix]) 

+ angleFeedbackGain*norm(desiredAnglesfix]-actualAngles[ix]); 

Steer_Digit represents the actual digit value being applied to the servomotor. The 
rateReferenceTable function simply converts the inputed rate to digits or, in the case 
where the inputed rate is larger than 5.235, clips it to within limits. The function is given 
below; 



double rateReferenceTable(double desiredRate) 

{ 

double inRate, 

outDigit; 

inRate=new_abs(desiredRate); 

if (inRatc<= 5.234) 
outDigit = inRate*! 95.41 55 ; 
else 

outDigit=1023; 
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if (desiredRate< 0.0) 
outDigit = - outDigit; 

return outDigit; 



} 

The two K constants steerFeedbackGain and angleFeedbackGain were determined 
by trial and error. AngleFeedbackGain was first maintained at 0.0 while testing 
SteerFeedbackGain. SteerFeedbackGain was made very low at the outset and increased 
through each test until the vehicle displayed unusual behavior such as shaking while 
operating its steering function. At this point the value was lowered and then the value for 
angleFeedbackGain was increased in the same way. These values became optimal at 
100.0 and 1000.0 for steerFeedbackGain and angleFeedbackGain respectively. 



4. Wheel Testing 

It was discovered while operating the vehicle that wheel 4 would on occasion be 
very badly misaligned from the other 3 wheels. Even after repeated realignment it would 
not operate as the others. At suspect was the thought this problem was software related . 
To test this, a routine was inserted into the SRK driveMotors() function with a menu item 
on the user interface. This routine simply turned wheel 4 360 degrees in one direction 
until the wheel aligned read the encoders for angle position, paused one second and then 
turned it in the opposite direction. At each pause, the wheel position was displayed to the 
interface screen and recorded. The data obtained is presented below in Figure 5.12 for 10 
iterations for clockwise and counterclockwise rotation: 





Clockwise Rotation 


Counterclockwise Rotation 


1 


000.867 


360.390 


2 


000.878 


360.390 


3 


000.976 


360.363 


4 


000.933 


360.414 


5 


000.984 


360.371 


6 


000.992 


360.394 


7 


000.992 


360.453 
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Clockwise Rotation 


Counterclockwise Rotation 


8 


000.902 


360.394 


9 


000.996 


360.445 


10 


000.996 


360.476 



Figure 5.12: Wheel 4 data based on position of rest after direction of turn. 



The average values for rotation in both the clockwise and counter clockwise directions 
were 000.955 and 360.426. These were averaged for 20 iterations see Appendix O for 
the full data set. Even though this data was obtained while the wheel was in a free 
floating environment without added friction, it still proved that wheel 4 was operating 
within 1% PS. Therefore, from this result a conclusion is drawn that the problem is not 
software related in nature but maybe mechanical. 
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VI. MOTION MODES 



A. OVERVIEW 

Chapter II mentioned several modes of motions that can be exhibited by 
Shepherd. Due to the nature of Shepherd’s characteristics of having four wheels that can 
be independently operated with two degrees of freedom, this makes it possible to obtain 
three degrees of freedom motion. In this chapter we will discuss the modes of “Tornado” 
(complex motion), Joystick controlled motion and searching motion simulation. The 
other motion modes are encompassed by these motions. The code for the other motions 
are provided in the Appendices. Also, the Tangential mode will not be discussed but will 
be left for a future reasearch topic and implementation. However the ground work as 
well as the code is in place for implementation (Appendix D). 

Before embarking on this discussion on motion modes the theory or basis for 
motion control must be presented This control motion theory, as well as the figures to 
follow, was taken from the works of Professors Yutaka Kanayama and Xiaoping Yun 
[refs 23 & 24]. First a vehicle coordinate system is defined on a rigid body robot. A 
configuration q is defined as 

(p, Xj/) = ((x, y), \i/), 

where p is the positioning of the vehicle origin and psi is the heading orientation of the 
vehicle Xv-axis. Next in describing the motion of the vehicle’s configuration which is a 
function of time, the following is the definition: 

q(t) = (p(t), vi/(t)) = ((x(t), y(t)), \[/(t)), 

where p(t) is the translational component and \}/(t) is the rotational component of the 
vehicle motion. Figure 6.1 is an illustration of this configuration and motion. Because 
the vehicle possesses 2 dimensional positioning it can exhibit 3 degrees of freedom 
motion. This motion contains three variables of : 

• Translation speed — v(t) = V ((dx(t)/dt)“ + (dy(t)/dt)^), 
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• Motion direction - 0(t) = atan2(dy(t)/dt, dx(t)/dt), if v(t) > 0, 

• Rotational speed — co(t) = d\|/(t)/dt. 

Resulting in a motion description of: 

Q(t) = (v(t),e(t),co(t)). 




Figure 6.1: Configuration and Motion of a Rigid Body [Ref. 23]. 

As was mentioned in Chapter II, 3 typical vehicle motions are possible. The 3 
typical motions are illustrated in Figure 6.2. Part (a) represents an all too familiar motion 
exhibited by a normal automobile or bicycle and is referred to as tangential motion. This 
is characterized by the fact that the vehicles heading orientation is equal to its 
translational motion direction ( dv|/(t)/dt = 0(t)/d(t)). Part (b) depicts a motion called 
constant orientation where the vehicles heading orientation is constant or rotational speed 
of the vehicle is 0. And part (c) shows a complex motion which incorporates rotation 
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Figure 6.2: Typical vehicle motions [Ref. 23]. 



independently superimposed on a translational motion. This complex motion is the basis 
for the “Tornado” motion mode to be discussed. 



B. “TORNADO” MOTION 

In analyzing this motion mode, several more discriptions will have to be made 
clear. One is that of a point. A point is defined as 

Pi = (xi,yi)?i(0,0) 

described in the vehicle coordinate system. On the rotary vehicle it corresponds to a 
wheel . In the case of wheels 1-4, it would be (40,-40), (40,40), (-40, -40), and (-40.40). 
So we will have to evaluate how these wheels move while the vehicle is executing the 
input motion or Q. In order to evaluate this, the polar coordinate representation is 
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another description that is needed. This representation is defined as (p, a) and is 
represented as 



p = V(xi^ +yi^) and a = atan2(yi, xi). 

The subscript is a representation of the wheel number and can represent any wheel based 
on the wheel location in the vehicle coordinate system. Figure 6.3 is a representation of 
the composite motion of a point on a vehicle. 

e 
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Given the above, a current configuration and motion of 



q(t) = ((x(t), y(t)), \|i(t)), 

Q(t) = (v(t), e(t),(0(t)), 

in the global coordinate system, the x and y-components of vix(t) and Viy(t) in the global 
coordinate system can be determined mathematically [Ref. 23 p.3]. The motion speed 
vi(t) and direction 9i(t) in the global coordinate system, motion direction ©I'^ft) in the 
vehicle coordinate system, and rotation rate tOi'^ of pi or any wheel is; 

v,(t) = V( (vu(t))' + (v,y(t))2) 

0i(t) = atan2(viy(t), v,x(t)) 

e/(t) = e,(t)-V|i(t) 

0 ),'' = ((v^-0+pW)+vp(o(CL)+-0-)sin(6-\|/ a)+p(v°-*>o))cos((0-\}/-a))/vi^ - to 
^ is theta dot, ° is omega dot, and is motion speed dot. 

Corresponding to the above equations in SRK is the following code from Appendix D 
(movement.c): 



ro=whp[i].rho; 

ro2=ro*ro; 

beta=vehicle.heading+whp[i]. alpha; 
vlx = speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 
vly = speed*sin(theta)+(whp[i].rho*omega*cos(beta)); 
desiredSpeeds[i] = new_sqrt(vlx*vlx + vly*vly); 

if (new_abs(desiredSpeeds[i]) > 0.01) { 
desiredAnglcs[i] = atan2(vly,vlx) - vehicle. heading; 
wheelAngleV = motion.Theta - vehicle. heading - whp[i]. alpha; 
desiredAngleRatesfi] = ( (speed*speed*thetaDot + ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngle\^ 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngle\^ ) 
/( desiredSpeeds[i]* desiredSpeeds[i]) - omega; 
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desiredAnglesO[i] = desiredAngles[i]; 
desiredAngleRatesO[i] = desiredAngleRates[i]; 

} 

The above code is very straight forward. A direct correlation can be discerned 
from the theory to the implementation. The variables were named as closely as possible 
to match the theory presented. The resulting code provided the mathematical 
computation providing the resulting values from a rotation superimposed independently 
on a translation motion. It is not shown, but all variables with brakets enclosing an ‘i’ 
represent a wheel. The entire routine is enclosed in a for loop which is iterated four 
times. Therefore, a resulting value is computed and provide to each wheel’s servo for 
both driving and steering . 

A detailed proof of the rotation rate of the moving direction at a point can be 
found in Ref. 24. As was stated earlier, this motion control theory is from the work of 
Professors Kanayama and Yun. The authors simply implemented this theory in code and 
applied it to the rotary vehicle. 



C. JOYSTICK CONTROLLED MOTION 

As discussed in chapter IV the user interface is provided by a laptop that includes 
a selection menu of shepherd functions. There are many options (still expanding, a work 
in progress) on the menu. The two of concern here are options three (3) and four (4), 
straight motion by joystick and XY-motion by joystick. Actually the emphasis will be on 
option four because option three can be considered a logical subset of option four. 

The “driver” function discussed in the file movement.c is called every 10 
milliseconds. One of the key functions executed under driver is the call to another 
function named “bodyMotion”. The bodyMotion function is also located in movement.c. 
When a user selects option three or four from the menu a motion mode 3 or 4 is chosen. 

If the user chooses option four the motion mode is 4. The case the user chose causes the 
joystick to be read by the readJoyStick function (Appendix H, utils.c). This function 
reads the three ports (A, B and C) from the Intel 85C55 Parallel Port 1 (Taurus board) 
and converts them into an ASCII string (code segment below); 
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void readJoyStick(void) 

{ 

unsigned int i,index; 

unsigned char *ctrlPort=(unsigned char*)PI01_CTRL; 
unsigned char *dataPort= (unsigned char*)PI01_DATA; 
unsigned int pioPortl [3]; 
double a= 0. 1 , xx, yy, 22 ; 

*ctrlPort=0x9b; /* set all ports (A,B,C) into input mode (read only) */ 
index=10; /* position for x-digits in string JOYSTICK */ 

for (i=0;i<3;i++) 
pioPortl [i] = *(dataPort+i); 

XX = (double)pioPortl[0]- 128.0; 
yy = (double)pioPortl[l]-128.0; 
if (xx >= 0.0) 

XX = xx*xx/100; 
else 

XX = -xx*xx/ 100; 
if (yy >= 0.0) 
yy= yy*yy/100; 
else 

yy = -yy^yy/lOO; 

joyStick.x = a*(xx) + (1.0-a)*joyStick.x; 
joyStick.y = a*()y) + (1.0-a)*joySdck.y; 



if (pioPortl [2] ==0x03) 

setVME((unsigned char *)VME92 10,0x00); /* no button pressed */ 
else { 
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setVME ((unsigned char *)VME921 0,0x02); /* if any button pressed */ 

} 

} 

It should be noted that the joystick input integer range is from [-127,128]; the 
intersection of the ‘x’ and ‘y’ axis on the physical joystick defines the center (x=0, y=0). 
Once the port is read some data smoothing is done. Due to the sensitive nature of the 
inputs a parabolic function was added for control (this can be seen above with the 
manipulation of the xx and yy variable). The purpose of the parabolic function is to 
ensure that when the joystick input values are small (near zero, center on the physical 
joystick) the slope changes will be of minimal effect, however if the input values are 
large (away from the physical joystick center) the effect on velocity or steering will also 
be proportionally large. The smoothing is continued for because of the possibility of very 
quick slope changes in the data being read-in. The objects joyStick.x and joyStick.y 
receive values that are only 10 percent (a = 0.1 in the code segment) of the xx or yy value 
plus 90 percent of the previous value for xx or yy. The aforementioned smoothing 
techniques were developed based on the experiences of Professor Kanayama an the 
constant “a = 0.1” determined by testing for the “best” hand feel and response. 

Upon completion of the read and smoothing of the joystick data, these values (i.e., 
speed and theta) are passed to the wheelMotion function described in the Shepherd 
Motion Control Architecture (see code segment below); 

case 4: /* X-Y Motion by Joystick */ 

readjoyStickQ; /* ejm 19 July 97*/ 

speed = -joySdck.j^O.l; /* speed control, 0.1 determined by testing */ 
tbeta = -joyStick.x*0.02; /* steering control, 0.02 determined by testing */ 
if (theta > HPI) theta = HPl; 
if (theta < -HPl) theta = -HPl; 

/* omega = -joyStick.omega*0.1;*/ /* pending ejm 24 july 97 */ 
break; 
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The two “if’ a statements with the theta conditions above reflect the capability of 
the rotary vehicle to complete perpendicular driving and parking. Actually, the theta 
values (steering angle) of the Shepherd vehicle are unlimited, however they are 
constrained here for ease of use and control. 

D. SEARCHING MOTION 

The searching motion discussed here is based on the requirement to have a 
smooth technique that allows shepherd to evenly and precisely search an area for UXOs. 
The aforementioned search algorithm and its implementation are not trivial. The 
algorithm and simulation presented here are the results of a lifetime of work by Professor 
Kanayama. Professor Kanayama’s expertise in the areas of motion planning, motion 
design, vehicle kinematics, sensing, guidance, learning, environmental representation, 
and control architectures for autonomous vehicles was the major influence. Professor 
Kanayama’s work on the Yamabico-11 robot includes development of composite 
function, line tracking, circle tracking, and neutral switching technique [Ref. 7]. For the 
aforementioned search algorithm and simulation the composite function and line tracking 
technique will be used. 

The goal of this simulation is to show that if given an orientation for the vehicle 
body and a given path, that the path can be tracked smoothly and the vehicle orientation 
will also change to ensure area coverage of the path traveled. Why is this important? 
This is critical because the desire is for the vehicle to search the path in the most safe, 
smooth, and efficient manner. The first assumption is that the time required to move 
across a path is 10 milliseconds or 0.01 seconds. Secondly, an assumption can be made 
for the vehicle orientation (called psi here), psi starts at 3*PI/4.0. Along the path traveled 
the orientation or psi will move from 3*Pl/4.0 to PI/4.0 (having a net change of PI/2.0). 
From the aforementioned change in psi, the incremental change over time can be 
determined (this incremental change is called omega). Dividing the net difference in psi 
(Pi/2.0) derived omega by 10 milliseconds, resulting in an omega value of 0.1570796327. 
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A value of 40 centimeters per second was arbitrarily chosen for the vehicle velocity. For 
the simulation the initial vehicle body coordinates as x = 0, y = 0, and the vehicle 
orientation as shown above 3*PI/4.0. Also, the coordinates of the wheels must be known. 
Hence, we place the vehicle wheels on sides that are 80 centimeters in length (like the 
Shepherd vehicle). 
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The last item is the structure required supporting the simulation [Ref. M], below 
is a code segment to illustrate the aforementioned statements. 

double deltaTime = 0.01; 

double Vel = 40.0; 

double omega = -0.1570796327; 

typedef struct { 
double x; 
double y; } 

POINT; 

typedef struct { 

POINT Point; 
double Theta; 
double Kappa; 
double Psi; 

} 

CONFIGURATION; 

q_init.Point.x = 0.0; 
q_init.Point.y = 0.0; 
q_init.Theta = 0.0; 
q_init.Kappa = 0.0; 

q_init.Psi = 2.356219449; /* 3*PI/4.0 */ 
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/ /individual wheel coordinates 
qfrontR.Point.x = 40; /* wheell */ 
qfrontR.Point.y = -40; 

qfrontL.Point.x = 40; /* wheel2 */ 
qfrontL.Pointy = 40; 

qrearR.Point.x = -40; /* wheeB */ 
qrearR. Point. y = -40; 

qrearL.Point.x = -40; /* wheel 4 */ 
qrearL.Point.y = 40; 



q_xaxis.Point.x = 0.0; /* line to be tracked, initial configuration */ 
q_xaxis.Point.y = 40.0; 
q_xaxis.Theta = 0.0; 
q_xaxis.Kappa = 0.0; 

Another key element in the structure CONFIGURATION is theta. Theta is 
simply the angle to the path being tracked. For instance if the vehicle is tracking a line, 
when the vehicle move onto the actual line then Theta’s value goes to zero. The final 
element required for the simulation is the step size that is used for the motion. The step 
size in the simulation code is called deltaS, and is veBdeltaTime (or .4 centimeters per 
second). Armed with this knowledge a simplified discussion can take place (for detailed 
knov/ledge of the compose function, and line tracking see Professor Kanayama’s motion 
planning and kinematics notes [Ref. 7]). 
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The compose function is used to determine (using deltaS ) the next position of the 
vehicle body. Here two compose functions are being used, one that is for the wheels 
(Compose2) and another for the vehicle body (Compose). 

CONFIGURATION Compose(CONFIGURATION& ql, CONFIGURATIONS 
q2, CONFIGURATIONS q3, doubles s, doubles deltaTime) 

{ double x,y, 

sinTheta = sin(ql. Theta), 
cosTheta = cos (ql. Theta); 

X = ql.Point.x + q2.Point.x*cosTheta - q2.Point.y*sinTheta; 
y = ql.Point.y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 
q3.Point.x = x; 
q3.Point.y = y; 

q3 .Theta = ql. Theta + q2.Theta; 

q3.Psi = ql.Psi + (omega * deltaTime); /* how to handle move left/right? */ 
fprintf(f6,”%10.3f%10.3f%10.3f%10.3f%10.3f\n", 
s,q3.Point.x, q3.Point.y,q3.Theta, q3.Psi); 
return q3; 

} // end Compose 



CONFIGURATION Compose2(CONFIGURAT10NS ql, CONFIGURATIONS 
q2, CONFIGURATIONS q3) /*position */ 

{ double x,y, 

sinTheta = sin(ql.Psi), 
cosTheta = cos(ql.Psi); 

X = ql.Point.x 4 - q2.Point.x*cosTheta - q2.Point.}"*sinTheta; 
y = ql.Point.y 4 - q2.Point.x*sinTheta 4 - q2.Point.y*cosTheta; 
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q3.Point.x = x; 
q3.Point.y = y; 
return q3; 

} // end Compose2 

The Compose function contains several lines that are important for observations 
in our simulation. The calculation of q3, Theta, as mentioned before the data theta should 
grow in positive manner as the vehicle moves from its initial point to the line above it. 
Once the line that is being tracked has been reached the value for theta goes to zero. 
Secondly the calculation for psi shows that as the vehicle moves from the initial position 
to the end of the line being tracked, the value of psi will be decremented by 
omega*deltatime (note in the code omega is defined as a negative number [Appendix 
M]). The Compose2 function is important because it provides the ability to compose the 
body orientation (psi) with the x and y cooridinates using the previously defined step. 

In the actual simulation the values of theta were manipulated to ensure the vehicle 
would track the next line above (40 centimeters higher) on the next step through the loop 
(see the code segment below); 

q_xaxis.Point.y = q_xaxis.Point.y + 40.0; 
if(ix%2==0){ 
q_xaxis.Theta = PI; 
q.Theta = PI; 
omega — fabs(omega); 

}else{ 

q_xaxis.Theta = 0.0; 
q.Theta = 0.0; 
omega = -omega; 

The simulation proved successful based on the data provided in Appendix L. Figure 6.4 
is a graphical representation of the simulation data in Appendix L. Moreover, all the 
structures are in place in the SRK [Appendix J, Consolidated header files] to implement 
the sensing motion. If more time was available for this thesis the sensing motion would 
have been implemented. 
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Figure 6.4: Sensing motion simulation. 
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VII. CONCLUSIONS 



A. SUMMARY 

What was accomplished as a result of this research. A restatement of the goals or 
questions addressed of this is necessary. The thesis was to examine the following 
research areas: 

• What kinematics algorithms must be developed to support a vehicle with three 
degrees of freedom of motion? The aforementioned algorithms must support 
highly flexible, controlled, and precise motion. 

• What types of controls are required to ensure the optimal mix of driving and 
steering resources? Moreover, what must be done to ensure that all the 
resources complement? 

• How can the knowledge gained in the aforementioned research areas be used 
to develop searching motion? 

• How should the hardware and software systems be implemented to support 
the aforementioned goals? 

The kinematics algorithms developed to support a vehicle with 3 degrees of 
freedom of motion can be divided into 2 categories, 'low level' and 'high level'. The first 
category is 'low level'. The 'low level' algorithms are illustrated in Chapter IV and in 
Appendix E (motor.c). These 'low level' algorithms are concerned with taking inputs and 
passing the inputs to the hardware. The empirical results from the direct input and 
servomotor output can be seen in Chapter V. The 'high level' category considers desired 
body motion and its transformation into wheel motion (see Chapter IV and Appendix D). 
The driver function in movement.c binds the high and low level categories allowing for 
highly flexible, precise, and controlled motion of the rotary vehicle. 

The controls required for the optimal mix of driving and steering resources are 
developed in Chapter V. The algorithms and their implementation as described in 
Chapter V removes or lessens the effect of variance and disparities of servomotor output. 
This ensures an optimal mix of driving and steering resources and that resources are 
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utilized in a complimentary manner. Hence, the aforementioned algorithms gives the 
desired output within 1% error for performance specifications. 

The knowledge gained and the researched discribed in Chapters IV, V, and VI can 
be used to implement the required searching motion the rotary vehicle needs for the UXO 
effort. Discoveries and knowledge gained during development of the Tornado, 
Sinusoidal, and Joystick controlled motions; coupled with the searching motion 
simulation can be used to implement the tangential and searching motion (see 
Appendices C and D). Moreover, the structures are presently in place to support both 
tangential and searching motions in Appendix D. 

The hardware and software systems are implemented as described in Chapters III 
and rv. This hardware-software implementation is tightly coupled and ensures proper 
communication within the system. This communcation allows the users desired inputs to 
be translated into vehicle motion in real-time. 

B. LESSONS LEARNED 

The Shepherd rotary vehicle was designed by Professor Kanayama but built by 
Mitsubishi Heavy Industries in Japan. This resulted in product not to specification, and a 
lack of legible documentation. One aspect was that the rotary vehicle could not fit into 
the lab it was planned for. Because, the size of the robot was not correct. Also, the 
documentation provided was in Japanese, big problem. None of the authors could read or 
speak Japanese. This generated numerous faxes and telephone calls to Mitsubishi Heavy 
Industries requesting support and clarification. Numerous hours were lost in this 
endeavor. Moreover, some of the wiring diagrams did not match the actual wiring 
implementation on the rotary vehicle. 

The compiler used is GCC version 2.7.2. 1. It was good because it was freeware. 
However, for cross-compiler requirements of the Motorola 68040 processor it left a lot to 
be desired. Most notably, the lack of all basic libraries. All input-output functions and 
math functions had to be written by the authors. Again, taking an inordinate amount of 
time and effort. Here the authors were recreating the wheel, per se. Also, the unusual 
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handling of structure passing required the development of several work-arounds. 
Essentially, standard C-code writing had to be modified extensively. The Motorola 
68040 chip was designed to support code written for the Motorola 68020. This may 
been true but the authors determined that for math functions this was not the case. This 
also may be related to the compiler (switches). The authors contacted Motorola and 
Omnibyte Corporations for help in dealing with these matters with little help provided. 
In the majority of cases the help provided led to dead-ends. Technicians at Omnibyte 
recommended that a purchase of a compiler suitable for the M68040 be made. 

The Taurus Bug (firmware on the Taurus Board) provided excellent capabilities. 
However, a lot of time was required to use the tools. On many occasions numerous hours 
were spent looking through loops of assembly code. This would not have been the case 
with a modem debugger. 

Group cross-pollenation is essential for a project with multiple disciplines 
involved. For example, a low voltage problem on the rotary vehicle caused the CPU to 
slow or halt on some occasions. Many hours were spent trying to determine what was 
wrong with the C-coding and the problem was hardware. This may have been alleviated 
had there been an electrical engineer on the team. Problems like this were exacerbated 
by the documentation problem mentioned earlier. Another problem was the overheating 
of the boards on the VMEbus (including the CPU) resulting in system shutdowns as well 
as aberrations in CPU behavior. 



C. RECOMMENDATIONS FOR FUTURE RESEARCH 

Recommendations for future research should include: 

• Implementation of tangential motion, the rudiments of which are already in 
place. 

• Implementation of sensing motion which is required for the percise, accurate, 
and safe detection of UXO's. 

• Transitioning the SRK to the PC environment possibly using LINUX freeware 
as the real-time operating kemal. 
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• Implementation of a M68040 specific compiler with all libraries. 

• Addition of the robot arm and implementation of the a neural net and expert 
system for UXO identification. Considerations should be given to sensors 
such as a magnetometer, digital camera devices, ground penetrating radar, 
xray devices and GPS. 

• Implementation of wireless ethemet or hand held computing devices for 
control and monitoring of the rotary vehicle by a user who is not co-located. 

• Possible missions other than UXO. For instance, an unmanned scout vehicle 
or mobile sentry. 

It is obvious from the above recommendations that the possible uses of the rotary vehicle 
is extensive and varied. The highly flexible, precise, and controlled motion of the vehicle 
makes it an ideal platform for many ground applications. 
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APPENDIX A: SOURCE CODE (MAKEFILE) 

The following code was modified by: Professor Kanayama, Thorsten Leonardy, 

Edward Mays, and Ferdinand A. Reid. 

1 # 

2 # * 

3 #File: MAKEFILE * 

4 # * 

5 # Environment: GCC Compiler v2.7.2 * 

6 # Update: 02 February 1997 (Leonardy) * 

7 # 02 April 1997 (Ed Mays) * 

8 # 14 May 1997 (Leonardy, added utilsOSO.*) * 

9 # Name: Thorsten Leonardy * 

10# Purpose: Makefile for project S H E P H E R D . * 

11 # * 

12# Invoke: make comp (to generate code) * 

1 3 # make print (to print all files to printer apl in Sp-5 1 1) * 

14 # make clean (to clean directory from object files) * 

15# * 

16# 

17 

18 

19 comp: startup.o shepherd.o timer.o serial. o math.o utils.o utils030.o user.o \ 

20 motor.o movement.o 

21 Id -Ttext 0x10000 -Tdata 0x20000 -Tbss 0x30000 -Map shepherd.map -oformat 
srec \ 
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22 -o shepherd.TXT startup.o shepherd.o timer.o serial.o math.o utils.o \ 

23 utilsOSO.o user.o motor.o movement.o 

24 

25 shepherd.o ; shepherd.c 

26 gcc -c -m68040 -o shepherd.o shepherd.c 

27 

28 timer.o : timer.c 

29 gcc -c -m68040 -o timer.o timer.c 

30 

31 serial.o : serial.c 

32 gcc -c -m68040 -o serial.o serial.c 

33 

34 #servo.o : servo. c 

35 # gcc -c -m68040 -o servo.o servo. c 

36 

37 utils.o : utils.c 

38 gcc -c -m68040 -o utils.o utils.c 

39 

40 utils030.o : utils030.c 

41 gcc -c -m68040 -o utils030.o utils030.c 

42 

43 user.o : user.c 

44 gcc -c -m68040 -o user.o user.c 

45 

46 motor.o : motor.c 

47 gcc -c -m68040 -o motor.o motor.c 
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48 



49 movement. o ; movement.c 

50 gcc -c -m68040 -o movement.o movement.c 

51 

52 startup.o : startup.s 

53 as -o startup.o startup.s 

54 

55 math.o: math.c 

56 gcc -c -m68040 -o math.o math.c 

57 

58 

59 # This cleans out everything except the Makefile, 

60 # and source files 

61 clean:; rm -f *.o core 

62 

63 # This prints all source files to the printer apl in Sp-51 1 

64 print:; enscript -2r -g -Papl makefile shepherd.map shepherd.h shepherd.c \ 

65 user.h user.c utils. h utils.c utils030.c serial. h serial.c servo.h servo.c \ 

66 timer.h timer.c movement. h movement.c wheeldrive.h wheeldrive.c \ 

67 math.h math.c startup.s 

68 

69 # This prints all source files to the printer spl in Sp-527 

70 prspl:; enscript -2r -g -Pspl makefile shepherd.map shepherd.h shepherd.c \ 

71 user.h user.c utils. h utils.c utils030.c serial. h serial.c servo.h servo.c \ 

72 timer.h timer.c movement. h movement.c navigat.h navigat.c wheeldrive.h 
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wheeldrive.c \ 



73 math.h math.c motor.h motor.c startup. s 

lA 

13 ^********************************* 

16# End of makefile 
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APPENDIX B: SOURCE CODE (SHEPHERD.C) 



The following code was modified by; Professor Kanayama^ Thorsten Leonardy, 
Edward Mays, and Ferdinand A. Reid. 

1 /* * 

2 * * 

3 * File: SHEPHERD.C * 

4 * * 

5 * Environment: GCC Compiler v2.7.2 * 

6 * Last update: 29 January 1997 * 

7 * Name: Thorsten Leonardy * 

8 * Purpose: Provides the kernel for SHEPHERD. * 

^ * * 

10 * Compiled; >gcc -c -m68040 -o shepherd.o shepherd.c * 

1 1 * * 

12 * */ 

13 

14 #include "shepherd. h" /* general defines */ 

15 #include "movement.h" 

16 

17 

1 8 /* * 

19 * constant character strings * 



21 unsigned char JOYSTICK[26]= { 
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22 



27,91,49,56,59,54,48,72, 

23 120,61,48,48,48,32, 

24 121,61,48,48,48,32, 

25 122,61,48,48,48,0); 

26 



/* ESC [T'8' 


; '6' '0' H */ 


/* 'x=000 ' 


*/ 


/* 'y=000 ' 


*/ 


/* 'z=000' 


*/ 



27 unsigned int wheelEncoder[8]; 

28 unsigned char bcdString[25]= 

29 {24,27,91,48,50,59,52,48,72, 

30 48,48,48,48,48,48,48,48,48,48,48,48,48,48,48,48 } ; 

31 



32 /* clock contains the current date/time updated every second in ascii format */ 

33 /* it starts with a count byte, the cursor positioning sequence followed by */ 

34 /* the date-group and the time group in the form: */ 

35/* clock=" mm-dd-yy hh:mm:ss." where is part of count or ESC */ 

36 /* sequence. */ 

37 



38 /*unsigned char clock[27]={ */ 

39/* 25,27,91,48,49,59,54,52,72, */ /* ESC [ 'O' T ; '6"4' H */ 

40 /* 109,109,45,100,100,45,121,121,32, */ /* 'mm-dd-yy ' */ 

41 /* 1 04, 1 04,58, 1 09, 1 09,58, 1 1 5, 1 1 5,0 ) ;*/ /* 'hh:mm:ss' */ 

42 



43 void advanceCountO 



44 

45 

46 

47 



{ 

edCounter-H-; 

) 
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48 main() 

49 { 

50 initBoardsO; /* initialize boards */ 

51 initMovementO; /* initialize movement */ 

52 sioInitQ; /* initialize 68C681 DU ART for serial I/O */ 

53 timerStartO; /* initialize and start timer-5 fopr motion control */ 

54 /* setup68030();*/ /* setup OMNI Module 0 for serial I/O to VTIOO */ 

55 enableO; /* enable all interrupts, user mode */ 

56 user(); /* let user handle the main portion */ 

57 disable!); /* disable interrupts, supervisor mode */ 

58 

59 /* here goes downloading stuff for analysis ... (i.e. copy memory */ 

60 /* from Taurus Main memory to host computer (Laptop or SparcStation) */ 

61 

62 return; 

63 

64 } /* end of main() */ 

65 

66 
67 

68 /******************************************************************** 
69 End of shepherd.c 

4=*****:*:*:*:*4:************4:**********************5|s**5l:***:t:***5l::t:5l:5l:**5l::t:5f:*sJ:^ 

71 

72 

73 
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APPENDIX C: SOURCE CODE (USER.C) 

The following code was modified by: Professor Kanayama, Thorsten Leonardy, 
Edward Mays, and Ferdinand A. Reid. 

1 /* * 

2 * * 

3 *File: USER .C * 

4 * * 

5 * Environment: GCC Compiler \ 1.12 * 

6 * Last update: 18 February 1997 * 

7 * Name: Thorsten Leonardy * 

8 * Purpose: Provides the userpart for SHEPHERD. * 

9 * * 

10 * Compiled: >gcc -c -m68040 -o user.o user.c * 

] I * * 

1 2 * */ 

13 

14 #include "shepherd.h" /* general defines */ 

15 

16 

17/* * 

18 * Global variables for test program * 

19 * */ 

20 

21 extern unsigned char inPortA; /* defined in serial.c */ 

22 extern unsigned char vtl00xy[9]; /* ESC-Sequence for Cursor Position */ 
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23 extern unsigned char clrSCR[5]; /* ESC-Sequence for clear Screen */ 

24 extern unsigned char clrLine[6]; /* ESC-Sequence for clear line */ 

25 extern unsigned char prtSCR[4]; /* ESC-Sequence for print screen */ 

26 extern unsigned char cursorOFF[5];/* ESC-Sequence for cursor off */ 

27 

28 extern void gotoXY(int,int); 

29 extern int wheelEncoder[8]; /* defined in shepherd.c */ 

30 

31 

32 extern char bcdString[]; /* defined in shepherd.c *! 

33 

34 unsigned short velocity=0; 

35 

36 

37 /* * 

38 * constant character strings * 

39 * *! 

40 

41 #define MENU.LINES 24 

42 char *menu[MENU_LENES]={ 

43 "SHEPHERD Main Menu (Last Update: 27 Feb 97)\n\r", 

44 " 1 —5 — 0 — 5 — 0— -5 — 0 — 5 — 0 — 5— -0 — 5— -0 — 5 — 0 — 5 — 0\n\r", 



45 "\n\r" 

46 "Please choose; Diagnostics\n\r", 

47 " \n\r". 



48 /* character pressed, analize ... */ 
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49 "(l)Stop W, 

50 "(2) Straight Motion (Autonomous) \n\r", 

5 1 "(3) Straight Motion by Joystick \n\r", 

52 "(4) XY-Motion by Joystick \n\r", 

53 "(5) Rotate \n\r", 

54 "(6) Sinusoidal Motion \n\r", 

55 "(7) Tornado (External Center of Rotation)\n\r", 

56 "(8) Tornado (Internal Center of Rotation)\n\r", 



57 "(9) Tangential Motion \n\r", 

58 "(0) Exit Program \n\r", 

59 "(a) Tangential Motion(II) \n\r", 

60 "(t) Test wheel Angle 4 \n\r", 



61 " \n\r", 

62 " W, 

63 " \n\r", 

64 " W, 

65 " \n\r", 

66 " \n\r", 

67 \n\r', 

68 " " 

69 }; 

70 

71 

72 

73 

74 



83 



75 /* 

76 * displayMenuQ 

77 * 

78 * Environment: GCC Compiler \ 1 . 1.2 

79 * Last update: 27 January 1997 



80 * Name: Thorsten Leonardy * 

81 * Purpose: This function outputs a menu to the screen * 

82 * * 

83 * *! 



84 void displayMenu(void) 

85 { 

86 int i; 

87 sioOut(0,clrSCR); /* clear screen */ 

88 gotoXY(Ll); 

89 for(i=0;i<MENU_LINES; i-H-) 

90 sioOut(0,menu[i]); 

91 return; 

92 } 

93 

94 

95 

96 /* * 

97 * statusO * 

98 * * 

99 * Environment: GCC Compiler v2.7.2 * 

100 * Last update: 18 Febmary 1997 * 
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101 * Name; Thorsten Leonardy * 

102 * Purpose; This function outputs a status line at the bottom of the * 



103 * screen. * 

104 * */ 



105 void status(unsigned char *p) 

106{ 

107 gotoXY(24,l); /* position cursor */ 

108 sioOut(0,clrLine); /* clear line */ 

109 sioOut(0,p); /* print text */ 

110 return; 

111} 

112 

113 

1 14/* * 

115* convertBCDO * 

116 * * 

117* Environment: GCC Compiler v2.7.2 * 

118* Last update: 20 February 1997 * 

119* Name: Thorsten Leonardy * 

120 * Purpose: This function converts an unsigned integer to a BCD string * 

121 * of 16 characters. The value is right justified with leading * 

1 22 * zeros. * 

123 * */ 

124void convertBCD(unsigned char *s, unsigned int data) 

125{ 

126 inti=15; 
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127 



128 for (i=15; i>=0; i--) { /* write 16 Bytes, adjust integer to right of string */ 

129 *(s+i)=48+(data%10); 

130 data=data/10; 

131 } 

132 return; 

133} 

134 

135/* * 

136 * convertIntO * 

137 * * 

138 * Environment: GCC Compiler v2.7.2 * 

139 * Last update: 1 1 June 1997 * 

140 * Name: Thorsten Leonardy, Yutaka Kanayama * 

141 * Purpose: This function converts a signed integer to a BCD string * 

142 * of 16 characters. The value is right justified with leading * 

143 * zeros. * 

144 * */ 

145void convertInt(unsigned char *s, int data) 

146{ 

147 inti=15; 

148 

149 if (data >= 0) 

150 *s = 32; I* space for positive number */ 

1 5 1 else 

152 { 
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153 *s = 45; /* minus sign for negative number */ 

1 54 data = -data; 

155 } 

156 

157 for (i=15; i>=l ; i-) { /* write 16 Bytes, adjust integer to right of string */ 

158 *(s+i)=48+(data%10); 

159 data=data/10; 

160 ) 

161 return; 

162} 

163 

164 

165 

166/* * 

167 * wheelDriveO * 

168 * * 

169 * Environment: GCC Compiler v2.7. 2 * 

170 * Last update; 27 February 1997 * 

171 * Name; Thorsten Leonardy * 

172 * Purpose; This function drives the specified wheel with required * 

173 * velocity. * 

174 * * 

175 * *! 

176void wheelDrivel (unsigned short wheel, unsigned short velo) 

177{ 

178 unsigned short *servoOut=(unsigned short *)0xffff0482; /* analog out */ 
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179 unsigned int *servoControl=(unsigned int*)OxffffffOO; /* Data out */ 



180 

181 *servoOut=velo; /* set velocity first */ 

182 

1 83 if (wheel) 

1 84 * servoControl=0x00000004; 

185 else 

186 *servoControl=0x00000000; 

187 

188 return; 

189} 

190 

191 

192 

193 

194 

195/* * 

196 * updateWheelStatusO * 

197 * * 

198 * Environment: GCC Compiler v2. 7. 2 * 

199 * Last update: 27 February 1997 * 

200 * Name: Thorsten Leonardy * 

201 * Purpose: This function reads the current shaft encoder readings for * 

202 * all eight servo motors and outputs them to the screen. * 

203 * * 

204 * unsigned int wheelEncoder[8] - array to hold the shaft encoder readings * 
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205 * unsigned char *bcdString - string to hold converted encoder reading * 



206 * * 

207 * *! 

208void updateWheelStatus(void) 

209 { 

210 

2 1 1 unsigned short i,posx,posy; 

212 

213 readWheelStatus(wheelEncoder); /* read wheel status; File servo.c */ 

214 

215 posx=8; /* x-position on screen for reading motor 1 */ 

216 posy=40; I* y-position on screen for reading motor 1 */ 

217 



218 for (i=0; i<8; i++) { 

219 posx=8+i%4; 

220 posy=40+20*(i/4); 

221 bcdString[3]=48+posx/10; 

222 bcdString[4]=48+posx%10; 

223 bcdString[6]=48+posy/10; 

224 bcdString[7]=48+posy%10; 



/* position for x *! 

/* position for y */ 

/* convert tens to ascii */ 

/* convert ones to ascii */ 
/* convert tens to ascii *! 

I* convert ones to ascii */ 



225 convertBCD(bcdString+9,wheelEncoder[i]); /* convert reading itself */ 

226 WRrTE_ENCODER(); /* output ascii */ 

227 ) /* end of for */ 

228 

229 return; 

230 } 
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231 



232 

233void displayDriveSpeedO 

234 { 

235 double speed00,speed0,speedl,speed2,speed3; 

236 

237 disableQ; 

238 speed00=desiredSpeeds[0]; 

239 speedO=Display_Speeds[0]; 

240 speedl=Display_Speeds[l]; 

241 speed2=Display_Speeds[2]; 

242 speed3=Display_Speeds[3]; 

243 Display_Speeds[0]=0.0; 

244 Display_Speeds[l]=0.0; 

245 Display_Speeds[2]=0.0; 

246 Display_Speeds[3]=0.0; 

247 

248 enableO; 

249 

250 convertInt(bcdString+9,speedOO); 

25 1 bcdString[3]='0'; 

252 bcdString[4]=’3'; 

253 bcdString[6]='4'; 

254 bcdString[7]='0’; 

255 sioOut(0,bcdString); 

256 convertInt(bcdString+9,speedO); 
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257 bcdString[3]='0’; 

258 bcdString[4]='3’; 

259 bcdString[6]='6’; 

260 bcdString[7]=’0'; 

261 sioOut(0,bcdString); 

262 convertInt(bcdString+9, speed 1 ); 

263 bcdString[3]='0'; 

264 bcdString[4]=’4'; 

265 bcdString[6]='6'; 

266 bcdString[7]=’0'; 

267 sioOut(0,bcdString); 

268 convertInt(bcdS tring+9,speed2) ; 

269 bcdString[3]='0’; 

270 bcdString[4]=’5’; 

271 bcdString[6]='6'; 

272 bcdString[7]=’0’; 

273 sioOut(0,bcdString); 

274 convertInt(bcdString+9,speed3); 

275 bcdString[3]='0'; 

276 bcdString[4]='6’; 

277 bcdString[6]='6'; 

278 bcdString[7]='0'; 

279 sioOut(0,bcdString); 

280 return; 

281 } 

282 
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283 void displayDriveSteerO 

284 { 

285 double steerOO,steerO,steerl,steer2,steer3; 

286 

287 disableO; 

288 steer00=Steer_Digits[0]; 

289 /* steerOO=desiredAngleRates[0]; */ 

290 steerO=desiredAngleRates[0]* 1000; 

291 steerO=Display_Steers[0]; 

292 steerl=Display_Steers[l]; 

293 steer2=Display_Steers[2]; 

294 steer3=Display_Steers[3]; 

295 Display_Steers[0]=0.0; 

296 Display_Steers[ 1 ]=0.0; 

297 Display_Steers[2]=0.0; 

298 Display_Steers[3]=0.0; 

299 

300 enableO; 

301 

302 convertInt(bcdString+9,steer00); 

303 bcdString[3]='0'; 

304 bcdString[4]='3’; 

305 bcdString[6]='4’; 

306 bcdString[7]=’0’; 

307 sioOut(0,bcdString); 

308 convertInt(bcdString+9,steerO); 
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309 bedstring [3]=’0'; 

310 bcdString[4]='3'; 

311 bcdString[6]='6'; 

3 1 2 bcdString[7]='0'; 

3 1 3 sioOut(0,bcdString); 

3 1 4 convertInt(bcdS tring+9, steer 1 ); 

315 bcdString[3]='0'; 

316 bcdString[4]='4'; 

317 bcdString[6]=6’; 

318 bcdString[7]='0'; 

319 sioOut(0,bcdString); 

320 convertInt(bcdString+9,steer2); 

321 bcdString[3]='0’; 

322 bcdString[4]='5’; 

323 bcdString[6]=’6'; 

324 bcdString[7]='0'; 

325 sioOut(0,bcdString); 

326 convertInt(bcdString+9,steer3); 

327 bcdString[3]='0'; 

328 bcdString[4]='6’; 

329 bcdString[6]=’6'; 

330 bcdString[7]=’0'; 

331 sioOut(0,bcdString); 

332 return; 

333} 

334 
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335 void display AnglesO 

336 { 

337 double steerO, steer I,steer2,steer3; 

338 

339 disableQ; 

340 

34 1 steerO=actualAngles[0]* 1000*RadsToDegrees; 

342 steer l=actualAngles[l]*1000*RadsToDegrees; 

343 steer2=actualAngles[2]* 1000*RadsToDegrees; 

344 steer3=actualAngles[3]* 1000*RadsToDegrees; 

345 

346 enableO; 

347 

348 convertInt(bcdString+9, SteerO); 

349 bcdString[3]='0’; 

350 bcdString[4]=’3'; 

351 bcdString[6]=’6'; 

352 bcdString[7]='0’; 

353 sioOut(0,bcdString); 

354 convertInt(bcdString+9,steerl ); 

355 bcdString[3]='0’; 

356 bcdString[4]='4’; 

357 bcdString[6]='6'; 

358 bcdString[7]='0'; 

359 sioOut(0,bcdString); 

360 convertInt(bcdString+9,steer2); 
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361 bcdString[3]='0'; 

362 bcdString[4]=’5’; 

363 bcdString[6]='6'; 

364 bcdString[7]=’0'; 

365 sioOut(0,bcdString); 

366 convertInt(bcdString+9,steer3); 

367 bcdString[3]='0'; 

368 bcdString[4]='6'; 

369 bcdString[6]=’6'; 

370 bcdString[7]='0'; 

371 sioOut(0,bcdString); 

372 return; 

373} 

374void displayVehicleConfigO 

375 { 

376 double coordx, coordy, heading, kappa; 

377 

378 disableQ; 

379 

380 coordx = vehicle.coord.x; 

38 1 coordy = vehicle. coord. y; 

382 heading = vehicle. heading; 

383 kappa = vehicle. kappa; 

384 

385 enableO; 

386 
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387 convertInt(bcdString+9,coordx); 

388 bcdString[3]='0'; 

389 bcdString[4]=’3'; 

390 bcdString[6]='6'; 

391 bcdString[7]='0'; 

392 sioOut(0,bcdString); 

393 convertInt(bcdString+9,coordy); 

394 bcdString[3]=’0'; 

395 bcdString[4]=’4'; 

396 bcdString[6]='6'; 

397 bcdString[7]='0'; 

398 sioOut(0,bcdString); 

399 convertInt(bcdString+9,heading); 

400 bcdString[3]='0'; 

401 bcdString[4]='5'; 

402 bcdString[6]='6'; 

403 bcdString[7]='0'; 

404 sioOut(0,bcdString); 

405 convertInt(bcdString+9,kappa); 

406 bcdString[3]='0'; 

407 bcdString[4]='6'; 

408 bcdString[6]='6'; 

409 bcdString[7]='0’; 

410 sioOul(0,bcdString); 

41 1 return; 

412 } 
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413 /* 



414 * userQ * 

415 * * 

416 * Environment: GCC Compiler v2.7. 2 * 

417 * Last update: 18 February 1997 * 

4 1 8 * Name: Thorsten Leonardy * 

419 * Purpose: This function provides the user shell. * 

420 * * 

421 * *! 

422 

423 void user(void) 

424 { 

425 int a; 

426 char *s; 

427 unsigned int *servoControl=(unsigned int *)VME2170;/* test only */ 

428 displayMenuO; /* display menu */ 

429 do 

430 { 

431 inPortA-?'; /* reset character */ 

432 while(inPortA=='?'); /* wait for character to be typed in */ 

433 /* character pressed, analize ... */ 

434 switch(inPortA) 

435 { 

436 case T : if (mode != 5) /* Stop */ 

437 { 

438 modeOstate = 0; 
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439 

440 

441 

442 

443 

444 

445 

446 

447 

448 

449 

450 

451 

452 

453 

454 

455 

456 

457 

458 

459 

460 

461 

462 

463 

464 



mode = 1 ; 

while (modeOstate ==0) { } ; 

disableO; 

alignO; 

enableO; 



/* *servoControl=0x00429429; test by Ed */ 
modeOstate = 2; 

) 

else 

{ 

mode = 1 ; 
disableQ; 

al ign AfterRotateO ; 
enableO; 

/* *servoControl=0x00429429; test by Ed*/ 



} 

initMovementO; 

break; 

case '2' : mode = 2; /* Straight Motion (Autonomous) */ 
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465 

466 

467 

468 

469 

470 

471 

472 

473 

474 

475 

476 

477 

478 

479 

480 

481 

482 

483 

484 

485 

486 

487 

488 

489 

490 



break; 



case '3' : mode = 3; /* Straight Motion by Joystick */ 
break; 



case '4' ; mode = 4; /* X-Y Motion by Joystick */ 

/* for (a=0;a<100;a++){ */ 

I* while ((edCounter % 200 != 0) && (a != 100)){ */ 
/* displayAnglesO; */ 

/* } */ 

/* } */ 

break; 



case '5' : mode5state = 0; /* Rotate */ 
mode = 5; 
break; 

case '6' ; mode = 6; /* Sinusoidal Motion */ 
break; 

case 7' : mode = 7 ; /* Tornado (Center of Rot External) */ 
break; 

case '8' : mode = 8; /* Tornado (Center of Rot Internal) */ 
break; 
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491 

492 

493 

494 

495 

496 

497 

498 

499 

500 

501 

502 

503 

504 

505 

506 

507 

508 

509 

510 

511 

512 

513 

514 

515 

516 



case '9' : mode = 9; /* Tangential Motion */ 
initTangent(); 
while(l){ 

while(edCounter%200 != 0){ }; 
displayVehicleConfigO; 



break; 

case 'a' : mode = 10; /* Tangential Motion (II) */ 
break; 

case 't' : modeTstate = 0; /* Steering test mode */ 

I* Flag=l; initialized in movement.c */ 
mode = 1 00; 
while (1) 

{ 

oldFlag = Flag; 

while (Flag == oldFlag) { ) 

displayAnglesO; 

} 

break; 

default : break; 

} I* end of switch */ 

} while(inPortA!='0');/* end of while, exit with 'O' entered at keyboard */ 
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517 sioOut(0,clrSCR); /* clear screen */ 

518 sioOut(0,"\n\r\n\r"); /*somecr,lf */ 

519 

520 return; 

521 

522 while(l) 

523 { 

524 while(edCounter%200 != 0) { } ; 

525 /* displayJoyStickO; *! 

526 displayDriveSteerO; 

527 /* displayAnglesO; */ 

528 }; 

529 sioOut(0,cursorOFF); /* switch cursor off (no blink) */ 

530 

531} /* end of user() */ 

532 

533 

534 

535asm(" 

536 .even 

537 .text 

538 .globl _WRITE_ENCODER 

539 

540_WRITE_ENCODER: 

541 

542 pea.l _bcdString 
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543 



trap #15 
dc.w 0x0023 



544 

545 

546 

547 rts 
548"); 

549 

550 

551 



552/**************************************************************** 
553 End of user.c 

555 
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APPENDIX D: SOURCE CODE (MOVEMENT.C) 

The following code was modified by: Professor Kanayama, Thorsten Leonardy, 
Edward Mays, and Ferdinand A. Reid. 

1 #include "shepherd.h" 

2 #include "movement.h" 

3 #include "math.h" 

4 

5 I* * 

6 * Main * 

y * * j 

8 void driverO 

9 { 

10 readEncodersO; /* Read Drive/Steer Motors *! 

1 1 computeActualRatesO; 

12 /*accumulateDriveSpeed(); only for wheel speed displaying */ 

13 accumulateDriveSteerO; 

14 bodyMotionO; 

15 wheelMotionO; 

16 /* testDrivelO; */ 

17 driveMotorsO; 

18 advanceCountO; 

19 } 
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20 



21 

22 I* * 

23 * Initialize Movement: * 

24 * intialize Configuration and vehicle motion * 

25 * *! 

26 void initMovementO 

27 { 

28 int ix; 

29 

30 Flag = 1; 

3 1 oldMode = 0; 

32 mode = 1 ; 

33 Omega_Speed=0.0; 

34 testCounter=0; 

35 edCounter=0; 

36 pathLength=0.0; 

37 

38 K 1 [0]= 1 1 ,448;/*0<=speed<=5,*/ 

39 Kl[l]=l 1.500; 

40 Kl[2]= 11.496; 

41 Kl[3]=12.375; 
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42 

43 

44 

45 

46 

47 

48 

49 

50 

51 

52 

53 

54 

55 

56 

57 

58 

59 

60 

61 

62 

63 



K2[0]=l 1.500;/*5>speed<8*/ 



K2[l]= 11.500; 
K2[2]= 11.644; 
K2[3]= 12.000; 



K3[0]=l 1 .61 1 ;/*8>=speed<20*/ 
K3[l]=l 1.585; 

K3[2]=l 1.686; 

K3[3]=l 1.840; 

K4[0]= 1 1 .7 1 1 ;/*20>=speed<=70*/ 
K4[l 1=11.659; 

K4[2]=l 1.705; 

K4[3]= 11.727; 

K6[0]=l 1.710;/*70>speed<K5*/ 
K6[ 11=11.700; 

K6[21= 11.700; 

K6[31=l 1.715; 
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64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 

76 

77 

78 

79 

80 

81 

82 

83 

84 



DigitToCmDrive[0]= +0.001 1369287;/* driving constant cm/count = 
digitToRadDrive* 18.9cm*/ 

DigitToCmDrive[l]= -0.001 1369287; 

DigitToCmDrive[2]= +0.001 1369287; 

DigitToCmDrive[3]= -0.001 1369287; 

motion.Speed=0.0; 

motion.Theta=0.0; 

motion.Omega=0.0; 

radius = 100; 

vehicle.coord.x=0.0; 
vehicle.coord.y=0.0; 
vehicle. heading=0.0; 
vehicle.kappa=l /radius; 

ai[0] = 40.0; ai[ 1 ] = 40.0; ai[2] =-40.0; ai[3] =-40.0; 
bi[0] =-40.0; bi[ 1 ] = 40.0; bi[2] =-40.0; bi[3] = 40.0; 
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85 

86 

87 

88 

89 

90 

91 

92 

93 

94 

95 

96 

97 

98 

99 

100 

101 

102 

103 

104 

105 

106 



joyStick.x = 0.0; 
joyStick.y = 0.0; 

setupPolar(whp); 

for (ix =0; ix <ARRAY_SIZE; ix++){ 
PreviousCountSpeed[ix]=99999999; 
PreviousCountSteer[ix]=99999999; 
Display_Speeds[ix]=0.0; 

Display_Steers[ix]=0.0; 
actualAngles[ix]=0.0; 
desiredSpeeds[ix] = 0.0; 
desiredAngleRates[ix] = 0.0; 
desiredAngleRatesO[ix] = 0.0; 
desiredAngles[ix]=0.0; 
desiredAngles0[ix]=0.0; 

WheelDirActO[ix]= 1 .0e8; 

WheelDirAct[ix] = 0.0; 

WheelDirDes[ix] = 0.0; 

steerReadings[ix]=0.0; I* not used only testing */ 
driveReadings[ix]=0; 
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107 } 



108} 

109 

110 
111 /* 



1 12 * SetupPolar 

113 * 






1 14void setupPolar(polar whp[4]) 

115{ 

1 16 whp[0].rho = whp[l].rho = whp[2].rho = whp[3].rho = 56.5685425; 

117 /* distances = 40 * sqrt(2) */ 

118 whp[0]. alpha = -QPI; /* front right wheel 1 */ 

119 whp[ 1], alpha = QPI; /* front left wheel 2 */ 

120 whp[2].alpha = -3.0*QPI; /* rear right wheel 3 */ 

121 whp[3]. alpha = 3.0*QPI; /* rear left wheel 4 */ 

122 } 

123 

124 

125/* * 



126 * bodyMotion -- Updates Vehicle * 

127 * *t 

128void bodyMotionO 
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129 { 

1 30 double vO, omegaO, 

131 linSpeed= 4.0, 

132 linAcc = 1.0, 

133 rotSpeed=0.1, /* 0.05, */ 

134 rotAcc =0.025, /* 0.0125; */ 

135 RPI =QPI*1.5; /* 67.5 degrees */ 

136 double theta, omega, speed; 

137 

138 speed = motionO. Speed = motion. Speed; /* save the previous motion */ 

139 theta = motionO.Theta = motion.Theta; /* for computing derivatives */ 

140 omega = motionO.Omega = motion. Omega; 

141 

142 switch(mode){ 

143 case 1: 

144 if (modeOstate == 2) 

145 break; 

146 if ( (Speed_Digits[0] == 0) && (Speed_Digits[ 1 ] == 0) && 

147 (Speed_Digits[2] == 0) && (Speed_Digits[3] == 0) && 

1 48 (Steer_Digils[0] == 0) && (Steer_Digits[ 1 ] == 0) && 

149 (Steer_Digits[2] == 0) && (Steer_Digits[3] = 0)) 

1 50 modeOstate = 1 ; 
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151 

152 

153 

154 

155 

156 

157 

158 

159 

160 

161 

162 

163 

164 

165 

166 

167 

168 

169 

170 

171 

172 



/* allStopO; will be inserted later */ 

break; 
case 2: 

speed = min(speed + 2.0*DeltaT, 10.0); 
break; 

case 3: /* Straight Motion by Joystick */ 

readJoyStickQ; /* ejm 19 July 97 */ 

speed = -joyStick.y*0.1; 
theta = 0.0; 
omega = 0.0; 
break; 

case 4; /* X-Y Motion by Joystick */ 

readJoyStickO; /* ejm 19 July 97*/ 

speed = -joystick. y*0.1; /* speed control */ 

theta = -joystick. x*0.02; /* steering control */ 
if (theta > HPI) theta = HPI; 
if (theta < -HPI) theta = -HPI; 

/* omega = -joyStick.omega*0. 1 ;*/ /* 24 july 97 */ 
break; 
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173 

174 

175 

176 

177 

178 

179 

180 

181 

182 

183 

184 

185 

186 

187 

188 

189 

190 

191 

192 

193 

194 



case 5: 



if (mode5state == 1){ 
readJoyStickO; 
speed = -joyStick.y*0. 1 ; 

} 

break; 

case 6: /* sinusoidal motion *! 
speed = min(speed + linAcc*DeltaT, 10.0); 
speed = speed; 
if (speed == 10.0){ 
pathLength += DeltaT*speed; 

theta = 0.4 * sin(pathLength/20.0); /* sine curve motion */ 

} 

break; 

case 7: /* Tornado External */ 

speed = min(speed + 1.0*DeltaT, 8.0); 
if ( speed == 8.0) 

omega = min(omega + 0.0125*DeltaT, 0.1); /* radius = 80 cm */ 



break; 



195 

196 

197 

198 

199 

200 

201 

202 

203 

204 

205 

206 

207 

208 

209 

210 

211 

212 

213 

214 

215 

216 



case 8: 



/* Tornado Internal */ 



speed = min(speed + 1 .0*DeltaT, 8.0); 
if ( speed == 8.0) 

omega = min(omega + 0.025*DeltaT, 0.2); /* radius = 40 cm */ 

break; 

case 9: /* tangential motion */ 
tangentialMotionO; 
break; 

case 10: /* tangential motion (II) */ 
speed = min(speed + linAcc*DeltaT, 8.0); 
break; 

case 100: 
break; 

} 



if (mode != 9){ 
motion. Speed = speed; 
motion. Theta = theta; 
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217 



motion. Omega = omega; 



218 

219 vehicle.heading = vehicle.heading + motion. Omega*DeltaT; 

220 vehicle.coord.x = vehicle.coord.x + motion. Speed*DeltaT * cos(motion.Theta); 

221 vehicle.coord.y = vehicle.coord.y + motion.Speed*DeltaT * sin(motion.Theta); 

222 

223 speedDot=(motion. Speed - motionO.Speed)/DeltaT; 

224 thetaDot=(motion.Theta - motionO.Theta)/DeltaT; 

225 omegaDot=(motion. Omega - motionO.Omega)/DeltaT; 

226 } 

227} 

228 

229 

230/* * 

23 1 * wheelMotion * 

232 * — *! 

233 void wheelMotionO 

234 { /*the function that truly belongs here is in calculate.org */ 

235 int i; 

236 double v 1 x , v 1 y, v 1 y v 1 xRatio; 

237 double theta=motion. Theta, 

238 omega=motion. Omega, 
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239 speed=motion. Speed, 



240 Omega2=omega*omega, 

241 Omega3=Omega2*omega, 

242 beta,ro,ro2, 

243 wheel An gleV; 

244 

245 if (mode == 5){ /* rotate case */ 

246 switch(mode5state){ 

247 case 0; 

248 /* turn each wheel by +-PI/4 in 5 seconds */ 

249 desiredAngles[0] += QPIbySOO; /* = (PI/4)/500 *! 

250 desired Angles [ 1 ] -= QPIby500; 

25 1 desired Angles[2] -= QPIby500; 

252 desiredAngles[3] += QPIby500; 

253 if (desiredAngles[0] >= QPl) 

254 mode5state = 1 ; 

255 break; 

256 

257 case 1 : /* drive wheels to rotate body */ 

258 

259 



260 



desiredSpeeds[0] = +speed; 
desiredSpeeds[l] = -speed; 
desiredSpeeds[2] = -t-speed; 



261 

262 

263 

264 

265 

266 

267 

268 

269 

270 

271 

272 

273 

274 

275 

276 

277 

278 

279 

280 

281 

282 



desiredSpeeds[3] = -speed; 
break; 

) 

return; 

} 

for (i=0; i < 4; i++){ /* non-rotate case */ 

ro=whp[i].rho; 
ro2=ro*ro; 

beta=vehicle.heading+whp[i]. alpha; 
vlx = speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 
vly = speed*sin(theta)+(whp[i].rho*omega*cos(beta)); 
desiredSpeeds[i] = new_sqrt(vlx*vlx + vly*vly); 

switch(mode){ 
case 1: 
case 2: 
case 3: 

if (speed < 0.0) 

desiredSpeeds[i] = -desiredSpeeds[i]; 
if (new_abs(vlx) > 0.01 ){ 

V 1 y V 1 xRatio=v 1 y/v lx; 
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283 

284 

285 

286 

287 

288 

289 

290 

291 

292 

293 

294 

295 

296 

297 

298 

299 

300 

301 

302 

303 

304 



desired Angles[i] = atan(vlyvlxRatio) - vehicle.heading; 
wheel AngleV = motion.Theta - vehicle.heading - whp[i], alpha; 
desiredAngleRates[i] = 

( (speed*speed*thetaDot + ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 
/( desiredSpeeds[i]* desiredSpeeds[i]) - omega; 
desiredAnglesO[i] = desired Angles[i]; 
desiredAngleRatesO[i] = desiredAngleRates[i]; 

} 

else{ 

desiredAngles[i] = desiredAnglesO[i]; 
desiredAngleRates[i] = desiredAngleRatesO[i]; 

} 

break; 
case 4: 

if (speed < 0.0) 

desiredSpeeds[i] = -desiredSpeeds[i]; 
if (new_abs(v 1 x) > 0.0 1 ) { 

V 1 yv 1 xRatio=v 1 y/v 1 x; 
desiredAngles[i] = theta; 



116 



305 

306 

307 

308 

309 

310 

311 

312 

313 

314 

315 

316 

317 

318 

319 

320 

321 

322 

323 

324 

325 

326 



desiredAngleRates[i] = 0.0; 



desiredAnglesO[i] = desiredAngles[i]; 
desiredAngleRatesO[i] = desired AngleRates[i]; 

} 

else{ 

desiredAngles[i] = desiredAnglesO[i]; 
desiredAngleRates[i] = desired AngleRatesO[i]; 

} 

break; 

case 6: 
case 7: 
case 8: 
case 9: 

if (new_abs(desiredSpeeds[i]) > 0.01 ){ 
desiredAngles[i] = atan2(vly,vlx) - vehicle.heading; 
wheelAngleV = motion.Theta - vehicle.heading - whp[i], alpha; 
desiredAngleRates[i] = 

( (speed*speed*thetaDot + ro2*Omega3) 

+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos( wheelAngleV) ) 
/( desiredSpeeds[i]* desiredSpeeds[i]) - omega; 
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327 

328 

329 

330 

331 

332 

333 

334 

335 

336 

337 

338 

339 

340 

341 

342 

343 

344 

345 

346 

347 



desiredAnglesO[i] = desiredAngles[i]; 
desiredAngleRatesO[i] = desiredAngleRates[i]; 



} 

else{ 

desiredAngles[i] = desiredAnglesO[i]; 
desiredAngleRates[i] = desiredAngleRatesO[i]; 

} 

break; 
case 10; 

desiredSpeeds[i] = speed * 

(new_sqrt((ai[i]*vehicle.kappa)*(ai[i]*vehicle.kappa) 
+(l-bi[i]*vehicle.kappa)*(l-bi[i]*vehicle. kappa))); 
if (vehicle. kappa !=0.0){ 

desiredAngles[i] = atan2(bi[i],(vehicle.kappa-ai[i])); 

} 

else { desiredAngles[i] = 0.0;} 
desiredAngleRates[i] = 0.0; 
break; 
case 100: 
break; 

)/* end switch *! 
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348 }/* end for */ 

349} 

350 

351 

352 

353/* * 

354 * joystickMotionInterface ejm 19 June 97 * * 

355 * */ 

356void joysticlcMotionlnterfaceO 

357 { 

358 motion. Speed = joystick. y; /* convert x-position into double */ 

359 motion.Theta = joystick. x; /* convert y-poistion into double */ 

360 motion. Omega =0.0; /*motion. Omega =joyStick.w; not implemented yet;*/ 
361} 

362 

363/* * 

364 * tangentialMotion * 

365 * */ 

366void tangentialMotionO 

367 { 

368 double deltaTheta, deltax, deltay, Si, totalDistance, deltaDistance; 

369 int ix; 
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370 



371 deltax = 0; 

372 deltay = 0; 

373 for (ix = 0; ix < 4; ix++){ 

374 deltax = deltax + actualSpeeds[ix]*cos(actualAngles[ix]); 

375 deltay = deltay + actualSpeeds[ix]*sin(actualAngles[ix]); 

376 } 

377 

378 /^returns the linear distance the vehicle has travelled */ 

379 deltas = (DeltaT/4)*new_sqrt((deltax*deltax)+(deltay*deltay)); 

380 

38 1 /* returns the difference between the changes in the distance */ 

382 /* of the left and right wheels */ 

383 deltaTheta = 0.0; 

384 for (ix = 0; ix < 4; ix++){ 

385 Si = actualSpeeds[ix]*DeltaT; 

386 deltaTheta = deltaTheta + (sin(actualAngles[ix])/ai[ix] 

387 - cos(actualAngles[ix])/bi[ix])*Si; 

388 } 

389 deltaTheta = deltaTheta/4; 

390 

391 totalDistance += deltaS; I* Keeps track of the total distance traved by vehicle */ 
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392 



393 /* update the vehicle’s configuration based on the distance travelled */ 

394 /* during the last motion control cycle */ 

395 

396 vehicle.heading += deltaTheta; 

397 circularArc(deltaS, deltaTheta); 

398 composeO; 

399 

400 deltaDistance = DeltaT*motion. Speed; 

401/* vehicle.kappa += (steer()*deltaDistance);*/ 

402 vehicle.kappa = 0.0; 

403/* motion.Theta += deltaTheta; */ 

404/* motion.Theta = vehicle.heading; */ 

405 motion.Theta = 0.0; 

406/* motion. Omega = vehicle.kappa*motion. Speed; */ 

407 motion. Omega = 0.0; 

408 thetaDot = deltaTheta/DeltaT; 

409 speedDot = 0.0; 

4 1 0 omegaDot = 0.0; 

411 } 

412 

413 /*******************************************************************/ 
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414/* FUNCTION; circuIarArc() */ 

415/* PARAMETERS: Configuration length --the arc length */ 

416/* alpha —the end orientation */ 

417/* config —pointer to the resultant configuration */ 

418/* PURPOSE: Given the arc length and alpha, to calculate the final */ 

419/* configuration */ 

420/* RETURNS: Configuration: pointer to the final configuration */ 



421/* COMMENTS: The main purpose of this function is to be used in conjunction */ 
422/* with composeO to form a new nextQ. In this case, length would*/ 



423/* actually be delta-s and alpha would be delta-theta. */ 

424/* Circular_arc() would determine the configuration after the incre-*/ 

425/* mental move in the local coordinate system of the original */ 

426/* configuration. Then compose() would take the original */ 

427/* configuration (in global coordinates) and the incremental */ 



428/* configuration (in local coordinates) to determine the */ 

429/* incremental configuration in global coordinates. */ 

430/********************************************************************* / 

431 void 

432circularArc(double length, double alpha) 

433 { 

434 

435 double alpha2,alpha4; 
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436 



437 alpha2 = alpha*alpha; 

438 alpha4 = alpha2*alpha2; 

439 defineConfig((l- alpha2/6.0 + alpha4/ 120.0) * length, 

440 (0.5 - alpha2/24 + alpha4/720.0) * alpha * length, 

441 alpha, 0.0); 

442} 



444/* FUNCTION: defineConfigO */ 

445/* PARAMETERS: double x,y,theta,kappa —The values that define a */ 
446/* configuration */ 

447/* PURPOSE: To allocate nad assign a configuration */ 

448/* RETURNS: Configuration: a configuration */ 

449/* COMMENTS: Was called def_configuration() in MMLIO */ 



451 void 

452defineConfig(double x,double y,double theta,double kappa) 

453 { 

454 incrementalMotion.coord.x = x; 



455 incrementalMotion.coord.y = y; 

456 incrementalMotion. heading = theta; 

457 incrementalMotion. kappa = kappa; 
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458 } 



459 

460 

461/********************************************************************/ 
462/* FUNCTION: composeO */ 

463/* PARAMETERS: Configuration *first -- pointer to the first configuration */ 
464/* *second — pointer to the second configuration */ 



465/* PURPOSE: To calculate the composition of the first and second */ 

466/* configurations */ 

467/* RETURNS: Configuration: configuration which is the */ 

468/* composition of the first and second configurations */ 



469/* COMMENTS: A typical example of the usage of this function is to determine */ 
470/* the goal position of a configuration in global coordinates. In */ 

471/* such an example, the first argument would be the original */ 

472/* configuration and the second argument would be the goal */ 

473/* configuration in the original configuration's local coordinate */ 

474/* system. The resultant third argument would then be the goal */ 

475/* configuration in global coordinates. Was called compO in MMLIO */ 

476/* LAST UPDATE: 10/25/94 Chien-LiangChuang */ 

477/******************************************************************* / 
478void 
479compose() 
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480 { 

481 

482 double x,y, theta; 

483 double xx,yy,tt; 

484 

485 holdVehicle.coord.x = vehicle.coord.x; 

486 holdVehicle.coord.y = vehicle.coord.y; 

487 holdVehicle.heading = vehicle.heading; 

488 hold Vehicle. kappa = vehicle.kappa; 

489 

490 X = incrementalMotion.coord.x; 

491 y = incrementalMotion.coord.y; 

492 theta = holdVehicle.heading; 

493 

494 

495 XX = cos(theta) * x - sin(theta) * y + holdVehicle.coord.x; 

496 yy = sin(theta) * x + cos(theta) * y + holdVehicle.coord.y; 

497 

498 tt = holdVehicle.heading + incrementalMotion. heading; 

499 

500 vehicle.coord.x = xx; 

501 vehicle.coord.y = yy; 
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502 vehicle.heading = tt; 

503 vehicle.kappa = holdVehicle.kappa; 

504} 

505 

507/* FUNCTION : steer(robot,Hne) PURPOSE ; evaluate steering */ 

508/* function */ 

509/*****************************************************************/ 
510double steer() 



511 



512{ 



513 double lambda, angle, dist; 



514 



515 if (currentPath.config. kappa == 0.0) 



5 1 6 lambda = - currentPath.a * vehicle.kappa 



517 



- currentPath.b * norm( vehicle. heading - currentPath.config.heading) 



518 



- currentPath.c *(-(vehicle. coord. x - currentPath.config.coord.x) 



519 



sin(currentPath.config.heading) 



520 



+(vehicle.coord.y - currentPath.config.coord.y) 



521 



cos(currentPath.config. heading)); 



522 else 



523 { 
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524 angle = Psi(vehicle. coord, currentPath.center); 

525 dist = distance(currentPath.center, vehicle.coord); 

526 if (currentPath.config.kappa > 0.0) 

527 { 

528 lambda = - currentPath.a * (vehicle.kappa-currentPath.config.kappa) 

529 - currentPath.b * nonn(vehicle.heading-(angle-HPI)) 

530 - currentPath.c * (currentPath.radius - dist); 

531 } 

532 else 

533 lambda = - currentPath.a * (vehicle.kappa-currentPath.config.kappa) 

534 - currentPath.b * norm(vehicle.heading-(angle+HPI)) 

535 - currentPath.c * (currentPath.radius + dist); 

536 } 

537 return lambda; 

538} 

539 

540/* 

541 void constantsO 

542 { 

543 double k; 

544 

545 k = 1 .0/sigma; 
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546 currentPath.a = 3.0*k; 



547 currentPath.b = 3.0*k*k; 

548 currentPath.c = k*k*k; 
549} 



551/* Function: Psi_function() */ 

552/* Purpose: Computes the Psi function of two given points */ 

553/* Parameters: point pi, p2 */ 

554/* Returns: double */ 

555/* Comments: */ 



557double 

558Psi(point pi, point p2) 

559 

560 { 

561 if ( p2.y - p 1 .y == 0.0 && p2.x - p 1 .x = 0.0) 

562 return 0.0; 

563 else 

564 return atan2(p2.y - p 1 .y, p2.x - p 1 .x); 

565} 

566 

567 
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568 /*******************************************************************/ 



569/* Function: distance() */ 

570/* Purpose: Computes the distance between two given points */ 

571/* Parameters: point pi, p2 */ 

572/* Returns: double */ 

573/* Comments: */ 



575double 

576distance(point pi, point p2) 

577 

578 { 

579 double X,Y; 

580 

581 X = pl.x-p2.x; 

582 Y = pl.y-p2.y; 

583 return new_sqrt( X*X + Y*Y ); 

584} 

585 

586void initTangentO 

587 { 

588 currentPath.config. coord. X = 0.0; 

589 currentPath.config.coord.y = 0.0; 
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590 currentPath.config.heading = 0.0; 

591 currentPath.config.kappa = 0.0; 

592 currentPath.radius = 0.0; 

593 currentPath.center.x = 0.0; 

594 currentPath.center.y = 0.0; 

595 sigma = 20.0; 

596 constantsO; 

597 

598 motion.Speed = 10.0; 

599 motion.Theta = 0.0; 

600 motion.Omega = 0.0; 

601 

602 vehicle.coord.y = 0.0; 

603 vehicle.coord.x = 0.0; 

604 

605 
606) 

607 
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APPENDIX E: SOURCE CODE (MOTOR.C) 

The following code was modified by: Professor Kanayama, Thorsten Leonardy, 

Edward Mays, and Ferdinand A. Reid. 

1 /*============================================================ 

2 // Edward Mays 

3 // Shpeherd project 

4 // 20 February 1997 

5 // MotionControl 

6 // ========================================================*/ 

7 

8 #include "motor.h" 

9 #include "shepherd.h" 

10 #include "math.h" 

11 

1 2 void readEncodersO { 

1 3 readDriveEncoders(driveReadings); 

1 4 reads teerEncoders(steerReadings); 

15 } 

16 

17/* */ 

1 8 /* Verifies validity of incoming speeds/angles and converts *! 

19 /* digitial input for the DA board */ 

20 /* */ 

21 void driveMotors(){ 

22 

23 int ix,Speed_Digit,Steer_Digit, counter; 
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24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 



double speed 1, steer 1, temp; 



unsigned short bitMask=0x8000; /* access bit 15 for align wheel 1 */ 
unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 

bitMask = bitMask » 3; 

/* updateWheelDriveO; wheel values for driving */ 

/* updateWheelSteerO; */ 

/* comupte the current actual wheel direction in WheelDirAct[] */ 

if (mode != 100){ 

for(ix =0; ix <ARRAY_SIZE; ix++){ 

^ ^ J.J j j-jg ^ OT1 

/* here +/- 1/50 of the steering value is added to the driving */ 

/* for each specified wheel. Note the negative sign on elements [1] *! 

/* and [3]provide the same direction driving as elements [0] and [2] *! 

Omega_Speed = desiredSpeeds[ix] + 

SteerDriveInteract*desiredAngleRates[ix]*WheelRadius; /* cm/sec */ 

/* conversion to digits */ 

Speed_Digit = velocityReferenceTable(Omega_Speed,ix) + 
DriveFeedBackGain*(Omega_Speed - actualSpeeds[ix]); 

Steer_Digit = rateReferenceTable(desiredAngleRates[ix]) 

+ steerFeedbackGain*(desiredAngleRates[ix]-actualAngleRates[ix]) 
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50 

51 

52 

53 

54 

55 

56 

57 

58 

59 

60 

61 

62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 



+ angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]); 



if (Speed_Digit>DigitsHigh) /* Limitation */ 

Speed_Digit= DigitsHigh; 
if (Steer_Digit>DigitsHigh) 

Steer_Digit= DigitsHigh; 
if (Speed_Digit<DigitsLow) 

Speed_Digit= DigitsLow; 
if (Steer_Digit<DigitsLow) 

Steer_Digit= DigitsLow; 

switch(mode) { 
case 2: 
case 3: 
case 4: 
case 5: 
case 6; 
case 7: 
case 8: 
case 9: 
case 10: 

Speed_Digits[ix]= (short)Speed_Digit; /* casting to short */ 

Steer_Digits[ix]= (short)Steer_Digit; 

break; 



case 1: 



133 



76 

77 

78 

79 

80 

81 

82 

83 

84 

85 

86 

87 

88 

89 

90 

91 

92 

93 

94 

95 

96 

97 

98 

99 

100 

101 



speed 1 = Speed_Digits[ix]; 
steerl = Steer_Digits[ix]; 
if ( speed 1 > 0) speed 1-; 
if ( speed 1 < 0) speed 1++; 
if ( steerl > 0) steerl—; 
if ( steerl < 0) steer 1++; 
Speed_Digits[ix] = speed 1; 
Steer_Digits[ix] = steerl; 
break; 

} /* end switch */ 

} /* end for */ 

} /* end if *! 
else { 

for (ix=0; ix<3; ix++){ 
Steer_Digits[ix] = 0; 

} 

for (ix=0; ix<4; ix++){ 
Speed_Digits[ix] = 0; 

} 



switch(modeTstate) { 
case 0: 

Steer_Digits[3] = 50*Flag; 
modeTstate = 1 ; 
break; 
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102 

103 

104 

105 

106 

107 

108 

109 

110 

111 

112 

113 

114 

115 

116 

117 

118 

119 

120 

121 

122 

123 

124 

125 

126 

127 



case 1: 



modeTstate = 2; 
break; 

case 2: 

modeTstate = 3; 
break; 

case 3: 

modeTstate = 4; 
break; 

case 4; 

modeTstate = 5; 
break; 

case 5: 

modeTstate = 6; 
break; 

case 6: 

modeTstate = 7; 
break; 



case 7: 
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128 

129 

130 

131 

132 

133 

134 

135 

136 

137 

138 

139 

140 

141 

142 

143 

144 

145 

146 

147 

148 

149 

150 

151 

152 

153 



modeTstate = 8; 
break; 

case 8: 

modeTstate = 9; 
break; 

case 9: 

modeTstate = 10; 
break; 

case 10: 

modeTstate =11; 
break; 

case 1 1 : 

modeTstate =12; 
break; 

case 12: 

modeTstate = 13; 
break; 

case 13: 

modeTstate = 14; 
break; 
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154 

155 

156 

157 

158 

159 

160 

161 

162 

163 

164 

165 

166 

167 

168 

169 

170 

171 

172 

173 

174 

175 

176 

177 

178 

179 



case 14: 



modeTstate =15; 
break; 

case 15; 

modeTstate = 16; 
break; 

case 16: 

modeTstate =17; 
break; 

case 17: 

modeTstate =18; 
break; 

case 18: 

modeTstate = 19; 
break; 

case 19; 

if (bitMask&*servoStatus)/* read servo status, */ 
{ /*wait until wheel aligned */ 



Flag = -Flag; 
modeTstate = 20; 
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180 

181 

182 

183 

184 

185 

186 

187 

188 

189 

190 

191 

192 

193 

194 

195 

196 

197 

198 

199 

200 

201 

202 

203 

204 

205 



} 

break; 



case 20: 

Steer_Digits[3] = 0; 
modeTstate = 21; 
break; 

case 21: 

modeTstate = 22; 
break; 

case 22: 

modeTstate = 23; 
break; 

case 23: 

modeTstate = 24; 
break; 

case 24: 

modeTstate = 25; 
break; 

case 25: 

modeTstate = 26; 
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206 



break; 



207 

208 case 26: 

209 modeTstate = 27; 

210 break; 

211 

212 case 27: 

213 modeTstate = 0; 

214 break; 

215 

216 default : break; 

217 ) /* end switch */ 

218 }/* end else */ 

219 

220 driveSteer(Steer_Digits); 

221 driveSpeed(Speed_Digits); 

222 }/* end driveMotors */ 

223 

224 /* Wheel Driving function */ 

225 void drives peed(short Speed_Digits[]) { 

226 

227 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

228 short *servoOutI=(unsigned short* )(VME92 10+0x0082); /* Analog out */ 

229 short *servoOut3=(unsigned short*)(VME92 10+0x0086); /* Analog out test*/ 

230 short *servoOut2=(unsigned short* )(VME92 10+0x0084); /* Analog out test*/ 

231 short *servoOut4=(unsigned short*)(VME92 10+0x0088); /* Analog out test*/ 
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232 



233 unsigned int wheelSelect=0x00924924; /* select all wheels for driving or steering */ 

234 

235 *servoControl=wheelSelect; 

236 

237 *servoOutl= (-Speed_Digits[0])«4; 

238 *servoOut3= (-Speed_Digits[2])«4; 

239 *servoOut2= Speed_Digits[l] «4; 

240 *servoOut4= Speed_Digits[3] «4; 

241 

242 return; 

243 } /* drives peed */ 

244 

245 

246 

247/* Wheel Steering function */ 

248void driveSteer(short Steer_Digits[]){ 

249 

250 unsigned int *servoControl=(unsigned int *)VME2 170; /* Data Out */ 

251 short *servoOutl=(unsigned short*)(VME92 10+0x008 A); /* Analog out wheel 1*/ 

252 short *servoOut3=(unsigned short*)(VME9210+0x008E); /* Analog out wheel3*/ 

253 short *servoOut2=(unsigned short*)(VME9210+0x008C); /* Analog out wheel2*/ 

254 short *servoOut4=(unsigned short*)(VME921 0+0x0090); /* Analog out wheel4*/ 

255 

256 

257 
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258 /* select all wheels for driving or turning */ 

259 unsigned int wheelSelect=0x00924924; 

260 

261 *servoOutl= Steer_Digits[0]«4; /* a neg volt turns wheels clockwise *! 

262 *servoOut3= Steer_Digits[2]«4; /* a pos volt turns counter clockwise*/ 

263 *servoOut2= Steer_Digits[l]«4; 

264 *servoOut4= Steer_Digits[3]«4; 

265 *servoControl=wheelSelect; /* turn on selected servo motor */ 

266 

267 return; 

268 

269 } /* end of driveSteer */ 

270 

271 

272 

273/* Wheel stop function */ 

274void allStop(){ 

275 

276 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

277 /* short *servoOutl=(unsigned short*)(VME92 10+0x0084);*/ 

278 

279 /* deselect all wheels for driving and/or turning */ 

280 

28 1 *servoControl=0x00000000; /* turn off selected servo motor */ 

282 /* *servoOutl= 0.0;*//* temp, does not belong in this function */ 

283 initBoardsO; 
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284 return; 

285 

286} /* end of allStop */ 

287 

288 

289void wheelDriveO 

290 { int ix,a; 

291 double alpha[ARRAY_SIZE]={30, 0, 0, 0), beta[ARRAY_SlZE]={0.0, 0.0, 0.0, 0.0); 

292 

293 driveMotors(alpha,beta); 

294 

295 return; 

296}/*end wheelDrive */ 

297 

298 

299 

300void readDriveEncoders(unsigned long int array}]) 

301 { 

302 unsigned char *p=(unsigned char*) VMECTR 1 , c 1 , c2, c3; 

303 int ix; 

304 long int temp; 

305 

306 for (ix=0; ix<4; ix++) { /* read all four motors subsequentially */ 

307 

308 *(p+3)=0x03; /* load output latch from counter */ 

309 *(p+3)=0x01 ; /* control register, initialize two-bit output latch */ 
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310 

311 /* read three bytes for specific counter ix and save in status *! 

312 /* first access to Output Latch Register reads least significant */ 

313 /* byte first */ 

314 

315 cl =*(p+l)&0x00ff; 

316 c2 = *(p+l)&0x00ff; 

317 c3 = *(p+l)&0x00ff; 

3 1 8 array [ix] = ((unsigned int)c 1 )l ((unsigned int)c2 « 8) I 

319 ((unsigned int)c3 « 16); 

320 

32 1 p=p+4; /* increment pointer for next counter */ 

322 

323 

324 } 

325 return; 

326 } /* end of readDriveEncoders */ 

327 

328 

329int readSteerEncoders(unsigned long int array[]) 

330{ 

331 unsigned char *p=(unsigned char*)(VMECTRl + 0x0100), cl, c2, c3; 

332 int ix; 

333 

334 

335 for (ix=0; ix<4; ix++) { /* read all four motors subsequentially */ 
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336 

337 *(p+3)=0x03; /* load output latch from counter */ 

338 *(p+3)=0x01; /* control register, initialize two-bit output latch */ 

339 

340 

341 /* read three bytes for specific counter ix and save in status */ 

342 I* first access to Output Latch Register reads least significant byte first */ 

343 

344 cl = *(p+l)&0x00ff; 

345 c2 = *(p-H)&0x00ff; 

346 c3 = *(p-i-l) & OxOOff; 

347 array[ix] = ((unsigned int)cl)l ((unsigned int)c2 « 8) I 

348 ((unsigned int)c3 « 16); 

349 

350 

351 p=p-(4; /* increment pointer for next counter */ 

352 

353 } 

354 return; 

355 } /* end of readSteerEncoders */ 

356 

357 void displayDirectionsO 
358{ 

359 /*if (edCounter%10 == 0){ */ 

360 convertBCD(bcdString-i-9,(unsigned int)(WheelDirDes[2]*RadsToDegrees)); 

361 /*bcdString-t-9*/ 
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362 



convertBCD(bcdString+9,(unsigned int)edCounter); 

363 bcdString[3]='0’; 

364 bcdString[4]='3'; 

365 bcdString[6]='4'; 

366 bcdString[7]=’0'; 

367 sioOut(0,bcdString); 

368 

369 /* sioOut(0,clrLine); */ /* clear line */ 

370 

37 1 convertBCD(bcdString+9,(unsigned int)(WheelDirAct[2]*RadsToDegrees)); 

372 bcdString[3]='0'; 

373 bcdString[4]=’3’; 

374 bcdString[6]=’6’; 

375 bcdString[7]=’0’; 

376 sioOut(0,bcdString); 

377 
378) 

379 

380/* 2 May */ 

38 1 void displaySpeedO 

382 { 

383 

384 convertBCD(bcdString+9,(unsigned int)l); 

385 bcdString[3]='0'; 

386 bcdString[4]='3’; 

387 bcdString[6]='4‘; 
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388 bcdString[7]=’0’; 

389 sioOut(0,bcdString); 

390 

391 /* sioOut(0,clrLine); */ /* clear line */ 

392 

393 convertBCD(bcdString+9,(unsigned int)steerReadings[l]); 

394 bcdString[3]=’0'; 

395 bcdString[4]='3'; 

396 bcdString[6]=’6’; 

397 bcdString[7]=’0'; 

398 sioOut(0,bcdString); 

399) 

400 

40 1 void testDrive 1 () 

402 { 

403 desiredAngleRates[0] = 1 ; 

404 desiredAngleRates[ 1 ] = 1 ; 

405 desiredAngleRates[2] = 1; 

406 desiredAngleRates[3] = 1; 

407 desiredSpeeds[0] = 0; /* wheels 2&4 must have minus sign */ 

408 desiredSpeeds[l] = 0; /* wheels 2&4 must have minus sign */ 

409 desiredSpeeds[2] = 0; /* wheels 2&4 must have minus sign *! 

410 desiredSpeeds[3] = 0; /* wheels 2&4 must have minus sign *! 

41 1 
412} 

413 
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414/* 2 May */ 

415void testDriveO 
416{ 

417 double MM, RES, N= 1044548, C=0.001; 

418 



419sioOut(0,"Entering testDrive ...\n\r"); 

420 

421 



422if(20<(N - steerReadings[l])*C){ 

423 

424 MM=20.0; 

425 } 

426 else{ 



427 

428 MM=(N - steerReadings[l])*C; 

429 } 

430 

431 

432 if(MM>0){ 

433 

434 RES=MM; 

435 } 



436 else{ 



437 

438 RES=0.0; 

439 } 
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440 



441 

442 

443 /* RES=max(min(20,(N - steerReadings[0])*C),0); */ 

444 

445 sioOut(0, "Leaving testDrive ...\n\r"); 

446 desiredSpeeds[l] = -(RES); /* wheels 2&4 must have minus sign */ 

447 



448 return; 
449} 

450 

451 

452 

453 

454 



455 void compute ActualRatesO 

456 { 

457 



458int i; 

459double count,speed; 
460 



461 for(i=0; i<=3; i++) 

462 { 

463 if(PreviousCountSpeed[i] == 99999999) /* for derivative for speed */ 

464 actualSpeeds[i]= 0.0; 

465 else 
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466 actualSpeeds[i]= 

467 (convertDifference((driveReadings[i] - PreviousCountSpeed[i])) 

468 *DigitToCmDrive[i])/DeltaT; 

469 PreviousCountSpeed[i] = driveReadings[i]; 

470 

47 1 if(PreviousCountSteer[i] == 99999999) /* for derivative for steering */ 

472 actualAngleRates[i]= 0.0; 

473 else 

474 actualAngleRates[i]= 

475 (convertDifference((steerReadings[i] - PreviousCountSteer[i])) 

476 *digitToRadSteer)/DeltaT; 

477 PreviousCountSteer[i] = steerReadings[i]; 

478 } 

479} 

480 

481 

482 

483void accumulateDriveSpeedO 

484 { 

485 int i; 

486 

487for(i=0;i<=3;i++){ 

488 Display_Speeds[i] += actualSpeeds[i]; 

489 1 

490 return; 

491 } 
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492 



493 void accumulateDriveSteerO 

494 { 

495 int i; 

496 

497for(i=0;i<=3;i++){ 

498 Display_Steers[i] += 10*actualAngleRates[i]; 

499 actualAngles[i] += actual AngleRates[i]*DeltaT; 
500} 

501 return; 

502} 

503 

504/* added 1 5 may */ 

505 void displayDriveAngleO 

506 { 

507 

508 double angle, angle 1, angle2, angle3; 

509 angle = actualAngleRates[0] * 1000.0; 

510 anglel = actual AngleRates[l] * 1000.0; 

51 1 angle2 = actualAngleRates[2] * 1000.0; 

512 angle3 = actualAngleRates[3] * 1000.0; 

513 

514 if (edCounter%100==0){ 

5 1 5 convertInt(bcdString+9,(int)desiredAngleRates[0]); 

516 bcdString[3]=’0’; 

517 bcdString[4]='3'; 
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518 

519 

520 

521 

522 

523 

524 

525 

526 

527 

528 

529 

530 

531 

532 

533 

534 

535 

536 

537 

538 

539 

540 

541 

542 

543 



bcdString[6]=’4’; 
bcdString[7]=’0'; 
sioOut(0,bcdString) ; 
convertInt(bcdString+9,(int) angle); 
bcdString[3]=’0’; 
bcdString[4]='3'; 
bcdString[6]='6'; 
bcdString[7]='0’; 
sioOut(0, bedstring); 
convertInt(bcdString+9,(int) angle 1 ) ; 
bcdString[3]='0’; 
bcdString[4]='4'; 
bcdString[6]='6'; 
bcdString[7]='0’; 
sioOut(0,bcdString); 
convertInt(bcdString+9,(int) angle2); 
bcdString[3]='0’; 
bcdString[4]=’5’; 
bcdString[6]=’6’; 
bcdString[7]='0'; 
sioOut(0,bcdString); 
convertInt(bcdString+9,(int) angle3); 
bcdString[3]=’0'; 
bcdString[4]='6'; 
bcdString[6]='6’; 
bcdString[7]=’0’; 
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544 sioOut(0,bcdString);} 

545 

546 return; 

547} 

548 

549 

550 

551 double velocityReferenceTable(double desiredVelocity,int i) 

552 { 

553 double inVelocity, 

554 outVelocity; 

555 

556 inVelocity=new_abs(desiredVelocity); 

557 

558 if (inVelocity>=0.0 && in Velocity <=5.0) 

559 outVelocity = inVelocity*Kl [i]; 

560 

561 if (inVelocity>5.0 && inVelocity< 8.0) 

562 outVelocity = inVelocity*K2[i]; 

563 

564 if (inVelocity>=8.0 && inVelocity<20.0) 

565 outVelocity = inVelocity*K3[i]; 

566 

567 if (inVelocity>=20.0 && inVelocity<= 70.0) 

568 outVelocity = inVelocity*K4[i]; 

569 



152 



570 if (inVelocity>70.0 && inVelocity<K5) 

571 outVelocity = inVelocity*K6[i]; 

572 

573 if (inVelocity> K5) 

574 outVelocity=1023; 

575 

576 if (desiredVelocity< 0.0) 

577 outVelocity = - outVelocity; 

578 

579 return outVelocity; 

580} /* end velocityLookupTable */ 

581 

582 

583double rateReferenceTable(double desiredRate) 

584{ 

585 double inRate, 

586 outDigit; 

587 

588 /*outDigit = new_abs(desiredRate); *//* test only */ 

589 

590 inRate=new_abs(desiredRate); 

591 

592 if (inRate<= 5.234) 

593 outDigit = inRate* 195.4155 ; 

594 else 

595 outDigit=1023; 
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596 



597 

598 if (desiredRate< 0.0) 

599 outDigit = - outDigit; 

600 

601 return outDigit; 

602} 

603 

604 

605 

606 /*************************************************************** 

607 Function convertDifference() returns the difference between the new shaft 

608 encoder position and the old shaft encoder position. The shaft encoder values 

609 contain only 24 bits (OxOOOOOO-Oxffffff). The routine adjusts for the trans- 

610 ition from Oxffffff to 0x000000 and vice versa. 

611 **************************************************************** ! 

612 

61 Sint convertDifference(int value) 

614{ 

615 if(value < -0x800000) 

616 value &= OxOOffffff; 

617 else if(value >= 0x800000) 

618 value 1= OxffOOOOOO; 

619 

620 return value; 

621 ) 
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622 



623 /* * 

624 * * 

625 * File: S E R V O . C * 

626 * * 

627 * Environment: GCC Compiler v2.7.2 * 

628 * Last update: 30 January 1997 * 

629 * Name: Thorsten Leonardy * 

630 * Purpose: Provides the kernel for SHEPHERD. * 

631 * * 

632 * Compiled: >gcc -c -m68040 -o servo.o servo.c * 

633 * * 

634 * *! 

635 

636 

637 

638/* * 

639 * readWheelStatusO * 

640 * * 

641 * Environment: GCC Compiler v2.7.2 * 

642 * Last update: 20 February 1997 * 

643 * Name: Thorsten Leonardy * 

644 * Purpo.se; This function reads the wheels counter status. * 

645 * This routine makes use of the fact that arrays are stored * 

646 * in memory consecutively. * 

647 * 
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648 * array 

649 * 



points to the beginning of the array 'wheelEncoder'. 
*! 



* 



650void readWheelStatus(unsigned char *array) 



651 { 






652 


unsigned char *p=(unsigned char*)VMECTRl ; 


653 


int ix; 




654 






655 


for (ix=0; ix<8; ix-H-) { /* read all eight motors subsequentially */ 


656 






657 


*(p+3)=0x03; 


!* load output latch from counter */ 


658 


*(p+3)=0x01; 


!* control register, initialize two-bit output latch */ 


659 






660 


/* read three bytes for specific counter ix and save in status */ 


661 


/* first access to Output Latch Register reads least significant byte first *! 


662 


*(array+3)=*(p+l); 




663 


*(array+2)=*(p+l ); 




664 


*(array+l)=*(p+l); 




665 


*(array+0)=0; 




666 






667 


array+=4; 


!* point to next entry in wheelEncoder*/ 


668 


p=p+4; 


/* increment pointer for next counter */ 


669 






670 


if (ix==3) p=(unsigned char*)VMECTR2; /* access the second VME Counter 


671 






672 


} 




673 


return; 
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674 } /* end of readWheel Status */ 

675 

676 

611 1 * * 

678 * clearShaftEncoder(unsigned short motors) * 

679 * * 

680 * Environment: GCC Compiler v2.7.2 * 

681 * Last update: 04 March 1997 * 

682 * Name: Thorsten Leonardy * 

683 * Purpose: This function clears the selected shaft encoder. * 

684 * * 

685 * motors bit mask to select motors, eg. 0x042 selects motor 2 and 7 * 

686 * to be cleared. * 

687 * - */ 

688void clearShaftEncoder(unsigned short motors) 

689 { 

690 unsigned char *p=(unsigned char*)VMECTRl ; 

691 int ix; 

692 

693 for (ix=0; ix<8; ix++,motors/=2) { 

694 if (motors & 0x01) *(p+3)=0x04; /* clear respective counter */ 

695 p=p+4; /* access next pointer */ 

696 if (ix==3) p=(unsigned char*)VMECTR2; /* access the second VME Counter */ 

697 } 

698 return; 

699 } /* end of clearShaftEncoder *! 
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700 



701 

702/* 

703 * alignO 

704 * Environment: GCC Compiler 

705 * Last update: 07 August 1997 m 



706 * Name: Thorsten Leonardy ,Yutaka Kanayama, Ed Mays * 

707 * Purpose: This function will align SHEPHERD'S wheels such that all * 

708 * will point in the forward direction. It utilizes the hall * 

709 * sensors for each of the four wheels. Crucial parameters * 

710* are as follows: * 

7 ] I * * 

712 * *! 



713void align(void) 

714{ 

715 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out*/ 

716 unsigned short *servoOut=(unsigned short*)(VME92 10+0x008 A); /* Analog out */ 

717 unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 

718 unsigned short bitMask=0x8000, bitMaskl; /* access bit 15 for align wheel 1 */ 

719 unsigned int wheelSelect=0x00004000; /* select servo for turning wheel 1 */ 

720 int ix, notYet; /* just a counter */ 

721 

722 do { 

723 notYet = 0; 

724 bitMaskl = bitMask; 

725 for (ix=0; ix < 4; ix++) 
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726 { 

727 if (bitMaskl & *servoStatus) 

728 { 

729 Steer_Digits[ix] = 0; 

730 } 

731 else 

732 { 

733 Steer_Digits[ix] = 40; 

734 notYet++; 

735 } 

736 bitMaskl = bitMaskl » 1; /* select next status align bit */ 

737 } 

738 driveSteer(Steer_Digits); 

739 } while(notYet); 

740 *servoControl=0x00000000; /* disable all wheels */ 

741 return; 

742} /* end of align */ 

743 

744/* * 

745 * alignAfterRotateO * 

746 * Environment; GCC Compiler * 

747 * Last update: 07 August 1 997 m * 

748 * Name: Thorsten Leonardy.Yutaka Kanayama, and Ed Mays * 

749 * Purpose: This function will align SHEPHERD'S wheels such that all * 

750 * will point in the forward direction. It utilizes the hall * 

751 * sensors for each of the four wheels. Crucial parameters * 
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752 * 



are as follows: 



753 * * 

754 * *! 

755void alignAfterRotate(void) 

756 { 

757 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out*/ 

758 unsigned short *servoOut=(unsigned short*)(VME9210+0x008A); /* Analog out */ 

759 unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 

760 unsigned short bitMask=0x8000, bitMaskl; /* access bit 15 for align wheel 1 */ 

761 unsigned int wheelSelect=0x00004000; /* select servo for turning wheel 1 */ 

762 int ix, notYet; /* just a counter */ 

763 

764 do { 

765 notYet = 0; 

766 bitMaskl = bitMask; 

767 for (ix=0; ix < 4; ix++) 

768 { 

769 if (bitMaskl & *servoStatus) 

770 { 

771 Steer_Digits[ix] = 0; 

772 } 

773 else 

774 { 

775 if(ix==l llix=2) 

776 Steer_Digits[ix] = 40; /* for wheel 1 and 2, rotate CCW */ 

111 else 
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778 



Steer_Digits[ix] = -40; /* for wheel 0 and 3, rotate CW */ 

779 notYet++; 

780 } 

781 bitMaskl = bitMaskl » 1; /* select next status align bit */ 

782 } 

783 driveSteer(Steer_Digits); 

784 } while(notYet); 

785 *servoControl =0x00000000; /* disable all wheels */ 

786 return; 

787 } /* end of align */ 

788 

789 

790/* * 

791 * align Wheel s() * 

792 * Environment: GCC Compiler * 

793 * Last update: 07 January 1997 * 

794 * Name: Thorsten Leonardy * 

795 * Purpose: This function will align SHEPHERD’S wheels such that all * 

796 * will point in the forward direction. It utilizes the hall * 

797 * sensors for each of the four wheels. Crucial parameters * 

798 * are as follows: * 

799 * * 

800 * servoControl Base address for the channels controling the servo motors * 

801 * switch servos on an off by accessing this address. * 

802 * Each servo is controlled by three bits: * 

803 * bits 0..2 -> driving wheel 1 * 
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804 * 

805 * 

806 * 

807 * 

808 * 

809 * 

810 * 
811 * 
812 * 



3. . 5 -> driving wheel 2 

6. . 8 -> driving wheel 3 

9.. 1 1 -> driving wheel 4 

12.. 14-> turning wheel 1 

15.. 17 -> turning wheel 2 

1 8.. 20 -> turning wheel 3 

21. . 23 -> turning wheel 4 

24.. 31 -> not used 



8 13 * servoOut Base address for the analog output card controlling the * 

814 * speed of the servos. Only the highest 12 bits are used. * 

0x0010 -> selects lowest positive velocity * 

0x7ff0 -> selects highest positive velocity * 

OxfffO -> selects lowest negative velocity (i.e -1 m/s) * 
0x8000 -> selects highest negative velocity (i.e. -lOOOm/s) * 



815 * 

816 * 

817 * 

818 * 

819 * 

820 * 
821 * 
822 * 
823 * 



It has been found that the MSB does not work properly. * 
Therefore, the velocities should lie within 1 1 bit range, * 
-1024 <= velocity <= +1023 * 



824 * servoStatus Base address for reading the servo status 

825 * The alignment bits are; Port B, Bit 15 for wheel 1 

826 * Bit 14 for wheel 2 * 

827 * Bit 13 for wheel 3 * 

828 * Bit 12 for wheel 4 * 

829 * * 
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830 * */ 

831 

832 

833void align Wheel s( void) 

834 { 

835 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

836 unsigned short *servoOut=(unsigned short*)(VME92 10+0x008 A); /* Analog out */ 

837 unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 

838 

839 unsigned short bitMask=Ox8000; /* access bit 15 for align wheel 1 */ 

840 unsigned int wheelSelect=Ox00004000; /* select servo for turning wheel 1 */ 

841 int wheel; /* just a counter */ 

842 

843 I* * 

844 * align wheels subsequentially, start with wheel 1 (front right) * 

845 * */ 

846 for (wheel= 1 ; wheel <5; wheel++) { 

847 



848 *servo0ut++=0x0200; /* set output value for servo first */ 

849 I* 0x0010 corresponds to smallest velocity */ 

850 *servoControl=wheelSelect; /* turn on selected servo motor */ 

85 1 while(!(bitMask&*servoStatus)); /* read servo status, wait until wheel aligned */ 

852 wheelSelect= wheelSelect«3; /* select next servo (motor) */ 

853 bitMask = bitMask » 1; /* select next status align bit */ 

854 } 

855 
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856 *servoControl=OxOOOOOOOO; /* disable all wheels */ 

857 

858 /* clearShaftEncoder(OxOff); */ /* clear all shaft encoders */ 

859 

860 /* sioOut(0,"aligned */ /* Output Message */ 

861 return; 

862 } /* end of alignWheels */ 

863 

864 

865 

866 

867 End of servo.c 

868 



1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 

21 

77 

23 

24 

25 

26 

27 

28 



APPENDIX F; SOURCE CODE (TIMER.C) 



I* * 

* * 

*File: TIMER .C * 

* * 



* Environment: GCC Compiler v2.7.2 

* Last update: 29 January 1997 



* Name: Thorsten Leonardy * 

* Purpose: Provides routines related to the AM9513 Timer Circuit, such * 

* as interrupt initialization, * 

* Compiled: >gcc -c -m68040 -o timer.o timer.c * 

* * 

* */ 



#include "shepherd.h" 
#include "timer.h" 



void timerStart(void) 

{ 



long ^vadr; 
unsigned char *p; 

short *ctrlPort = (short*) TIMER_CTRL; 
short *daiaPort = (short*) TIMER_DATA; 

/* initialize the interrupt counter */ 
intCounter=0; 
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29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 

50 

51 

52 

53 

54 

55 

56 

57 



/* load address for interrupt service routine */ 
vadr=(long*)VBA_TIMER; 

*vadr=(long)TimerHandler; 

/* Issue commands to set control and data register */ 

/* refer to Fig 1-20, 1-8, 1-12 *l 



*ctrlPort=Oxffff; /* Master reset, clear data registers */ 

*ctrlPort=0xff5f; /* load all counters */ 

*ctrlPort=Oxffef; /* Set MM 13 (Enter 16-bit bus mode) */ 

*ctrlPort=0xffl7; /* Select master mode register */ 
*dataPort=0xale0; /* set master mode register ... */ 

/* -I- > f == 1 sec interrupt interval */ 

/* e == 0. 1 sec *! 

/* d== 0.01 sec */ 

/* c== 0.001 sec */ 

*ctrlPort=0xff05; /* Select CMR timer 5 ... */ 

/* utilize Data Pointer Sequencing */ 

*dataPort=0x0e32; /* and write to counter mode register */ 

/* + > f= 10000 *! 

/* multiply value according to dataPort below e = 1000 *! 

I* by the factor set here to obtain timing... d= 100 */ 

I* c= 10 *! 

I* b = 1 */ 
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58 

59 

60 

61 

62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 

76 

77 

78 

79 

80 

81 

82 

83 

84 

85 

86 



/* to obtain the correct timing, multiply value determined in data-*/ 
/* port below by the factor given above. E.g. dataPort is set to */ 

/* 58 (corresponding to lOusec) and factor 1000 is chosen above, */ 



/* then the interrupt would occur every 10msec! */ 

*dataPort=58; /* load register, 58 -> lOusec */ 

/* 930 -> 1 msec */ 

*ctrlPort=0xff70; /* load and arm timer 5 */ 

p=(unsigned char*)ISM_TIMER; /* ISM Configuration for Timer */ 
*p=0xcb; /* assert LIRQ-3 to VIC */ 



p=(unsigned char*)VIC_LIRQ3; /* VIC LICR for LIRQ-3 from ISM */ 
*p=0x03; /* assert IRQ-3 from VIC to 68040 */ 

return; 

} /* end of timerStart */ 



y 5|c * * jjc :i: He *** ******************************* sic** ****5i: ****** 5|« *** * 

Assembler routines 

Hc*****H:*** *************************************************** *****y 



/* TimerHandler, its address is set from within timerStart */ 



asm(" 

.even 

.text 
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87 .globl _TimerHandler 

88 

89 _TimerHandler: 

90 

91 

92 link a6,#-184 /* alocate 184 Bytes on stack to save registers */ 

93 fsave a6@(-184) 

94 fmovemx fp0-fp7,sp@- /* move floating point registers 80 bit each */ 

95 fmovel fpcr,sp@- /* move floating point Control Regioster */ 

96 fmovel fpsr,sp@- /* move floating point status register *! 

97 fmovel fpiar,sp@- /* move floating point Instruction address register */ 

98 moveml d0-d7/a0-a5,sp@- /* save data and address registers (14*4 Byte) */ 

99 

100 

101 addq.l #0x0 1 ,_intCounter /* increment interrupt counter */ 

102 move.w #0xffe5,0xfff4 1 002 /* clear toggle out for timer 5 */ 

103 

104 moved #0xffff0081,al /* load VME9421 Status register */ 

105 eor.b #0x02,(al) /* toggle green indicator light to indicate timer */ 

106 /* for interrupt handling is working properly ... */ 

107 and.b #0xfe,(al ) /* turn red light on to indicate that motion control*/ 

108 /* will start (this will assert the SYSFAIL line on */ 

1 09 /* the VME-Bus, but we don't care at this point). */ 

110 

111 jsr _driver /* execute motion control part */ 

112 

113 

114 move.l #0xffff0081,al /* load VME9421 (digital out board) Status register */ 

115 or.b #0x0 1, (a 1) /* turn off red indicator light to indicate that */ 
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116 

117 

118 

119 

120 

121 

122 

123 

124 

125 

126 

127 

128 

129 

130 

131 

132 

133 



/* motion control is done. 



*/ 



moveml sp@+,d0-d7/a0-a5 
fmovel sp@+,fpiar 
fmovel sp@+,fpsr 
fmovel sp@+,fpcr 
fmovemx sp@+,fp0-fp7 
frestore a6@(-184) 
unlk a6 

rte 



End of timer.c 
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APPENDIX G: SOURCE CODE (MATH.C) 



The following code was modified by: Professor Kanayama, Thorsten Leonardy, 
Edward Mays, and Ferdinand A. Reid. 

1 /* * 

2 * * 

3 *File: MATH .C * 

4 * * 

5 * Environment: GCC Compiler v2.7.2 * 

6 * Last update: 1 7 March 1 997 * 

7 * Name: Thorsten Leonardy * 

8 * Purpose: A Simple Math library. * 

9 * */ 

10 

1 1 #include "shepherd.h" 

12 #include "math.h" 

13 

14 #define pio4 0.785398163 

1 5 #define pio2 1 .570796327 
16#definepi 3.141592654 
17#definepi2 6.283185307 
18 
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19 



20 

22 FUNCTION: norm() 

23 PARAMETERS: double angle — the angle to normalize 

24 PURPOSE: normalize the input angle between -PI and PI 

25 RETURNS: double: the normalized angle in radians 

26 COMMENTS; This is the most common normalizing function used in the system 

27 This performs that same as norm() and normalize)() in MMLIO. 

29 double norm(double angle) 

30 { 

3 1 while ((angle > pi) II (angle <= -pi)) 

32 { 

33 if (angle > pi) 

34 angle -= pi2; 

35 else 

36 angle += pi2; 

37 ) 

38 return angle; 

39 } 

40 
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41 



42 

43 

44 /* * 

45 * * 

46 * new_abs() * 

47 * * 

48 * Environment: GCC Compiler v2.7.2 * 

49 * Last update: 14 March 1997(mod 2 April 97 by Ed Mays) * 

50 * Name; Thorsten Leonardy * 

5 1 * Purpose: A function returning the absolute value of x. * 

52 * */ 

53 double new_abs(double x) 

54 { 

55 if (x>=0.0) 

56 return (x); 

57 else 

58 retum(-x); 

59 } 

60 
61 
62 
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63 



64 /* * 

65 * atan2() * 

66 * * 

67 * Environment: GCC Compiler v2.7.2 * 

68 * Last update: 17 March 1997 * 

69 * Name: Thorsten Leonardy * 

70 * Purpose: Computes tan(y/x) where x,y are real. If both variables are * 

71 * zero, atan2 returns zero. For any other values, atan2 will * 

72 * return the positive angle for the (x,y)-pair, e.g., * 

73 * (x,y)=(0,-l) would return atan2=3/2*pi ! * 

74 * ix determines the accuracy (highest order term in expansion)* 

75 * For the worst case, ly/xl close to one, ix should be very * 

76 * high. Here is some data: * 



77 


* 


Ix/yl 


ix 


accuracy of result [rad] 


* 


78 


* 


0.9 


101 


+- 1.88*10E-7 


* 


79 


* 




1001 


+- 1.57*10E-49 


* 


80 


* 


0.99 


101 


+-3.45*10E-3 


* 


81 


* 




1001 


+- 4.18*10E-8 


* 


82 


* 




10001 


+- 2.18*10E-48 


* 


83 


* 


0.999 


101 


+-8.76*10E-3 


* 


84 


4 : 




1001 


+- 3.65*10E-4 


* 
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85 * 



10001 +-4.50*10E-9 



86 * */ 

87 double atan2(double y, double x) 

88 { 

89 double erg=0.0, z=0.0, z2; 

90 int ix= 101, flag 1=0, flag2=0; 

91 

92 if ((new_abs(y)>new_abs(x))&&(y!=0)) 

93 { 

94 z=x/y; /* in case ly/xl>l compute atan(l/z) */ 

95 flagl=(y>0)-(y<0); /* a handy sign-function */ 

96 } 

97 else if (x!=0) 

98 { 

99 z=y/x; /* in case ly/xkl compute atan(z) */ 

100 flag2=(x<0.0); /* in this case need to add pi to final result */ 

101 ) 

102 

103 /* From here on Izl must always be less than one !!! */ 

104 z2=z*z; 

105 

106 /* Taylor expansion */ 
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107 if (new_abs(z)<1.0) { /* computation for ly/xkl */ 

108 while (ix>l) { 

109 erg=z2*(1.0/ix-erg); /* try alternatively for accuracy: (z2/ix)*(1.0-ix*erg) */ 

110 ix-=2; 

111 } 

112 erg=z-z*erg; 

113 } 

1 14 else erg=((z>0.0)-(z<0.0))*pio4; /* for ly/xl=l result is either +- pi/4 */ 

115 

116 if (flagl==l) erg=pio2-erg; /* point lies in 3rd or 4th octant for flagl=+l */ 

1 17 else if (flagl==-l) erg=-pio2-erg; /* ... or in 6th or 7th octant for flagl=-l */ 

118 if (flag2) erg=erg+pi; /* point lies in 4th or 5th octant */ 

119 /* if (erg<0.0) erg=erg+pi2; deleted 6/27/97 */ 

120 

121 retum(erg); 

122 } 

123 

124 

125/* * 

126 * atan() yk * 

127 * */ 

128double atan(double x) 
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129 { 

130 double erg=0.0, z=0.0, z2; 

131 int ix= 101, flag=0; 

132 

133 if(x==0.0) 

134 return (0.0); 

135 if (new_abs(new_abs(x)- 1 .0) < 0.00001) 

1 36 return (pio4 * x); /* return +- pi/4 */ 

137 if (new_abs(x) > 1.0) 

138 { 

139 z=1.0/x; /* in case lxl>l compute atan(l/x) */ 

140 flag=(x>0)-(x<0); /* a handy sign-function */ 

141 } 

142 else 

143 z=x; /* in case Ixkl compute atan(x) */ 

144 z2=z*z; /* From here on Izl is less than one ! ! ! */ 

145 /* Taylor expansion *! 

146 while (ix>l) 

147 { 

148 erg=z2*(1.0/ix-erg); 

149 ix-=2; 

150 } 
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151 



erg=z-z*erg; 

152 if (flag == 1) erg = pio2-erg; 

153 if (flag == -1) erg =-pio2-erg; 

154 retum(erg); 

155} 

156 

157 

158 

159/* * 

160 * cos() * 

161 * * 

162 * Environment; GCC Compiler v2.7.2 * 

163 * Last update: 17 March 1997 * 

164* Name: Thorsten Leonardy * 

165 * Purpose: Computes cos(x) where x can be any real number. 

1 66 * ix determines the accuracy (highest order term in expansion)* 

167 * */ 

168double cos(double x) 

169{ 

170 double erg; 

171 int quadrant, ix=20; /* ix must be an even number */ 

172 
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173 /* analyze and reduce x to the appropriate range ... */ 

174 quadrant=(x/pio2+(x>=0)-(x<0))/2; /* determine in what sector x is */ 

175 x=x-quadrant*pi; /* reduce x to region [-pi/2... pi/2]*/ 

176 x=x*x; /* compute x'^2 and store in x */ 

177 erg=1.0; 

178 

179 /* the cosine taylor computation is a one-liner ;-) */ 

180 while (ix>0) { 

181 erg=1.0-erg*x/ix/(ix-l); 

182 ix-=2; 

183 } 

184 

185 /* shift sign if quadrant is not 1,3,5,... */ 

186 if (quadrant%2) erg=-erg; 

187 

1 88 retum(erg); 

189} 

190 

191 

192 /* * 

193 * sin() * 

194 * * 
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195 * Environment: GCC Compiler v2.7.2 

196 * Last update: 14 March 1997 



197 * Name: Thorsten Leonardy * 

198 * Purpose: Computes sin(x) where x can be any real number. * 

199 * *! 



200double sin(double x) 

201 { 

202 retum(cos(x-pio2)); /* since sin(x)=cos(x-pi/2) */ 

203} 

204 

205 

206 

207/* * 

208 * sqrtO * 

209 * Ed Mays and Ferdinand Reid March 1997 ” 

210 * Environment; GCC Compiler v2.7. 2 ” 

211 * */ 

212double new_sqrt(double x) 

213 { 

214 double xl,x2; 

215 int count; 

216 
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217 



218 if (x == 1.0) retum(l.O); 

219 xl = 1.0; 

220 for (count=0; count < 10; count++){ 

221 x2 = .5*(xl +x/xl); 

222 xl = x2; 

223 ) 

224 

225 return (x2); 

226 } 

227 

228 

229 

230 

231/* * 

232 * new_sqrtl() * 

233 * Ed Mays and Kanayama * 

234 * Environment; GCC Compiler v2.7.2 * 

235 * */ 

236double new_sqrt 1 (double x) 

237 { 

238 double xl,x2; 
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239 xl = 1.0; 



240 x2=-1.0; 

241 while(new_abs(xl-x2) < l.Oe-9) 

242 { 

243 x2 = xl; 

244 xl = .5 * (x2 + x/x2); 

245 } 

246 

247 return (xl); 

248 } 

249 

250 

251 

252 

253/* ed move to math.c*/ 
254double min (double a, double b) 

255 { 

256 if (a <= b) 

257 return a; 

258 else 

259 return b; 

260 } 
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261 



262/* ed move to math.c*/ 
263double max (double a, double b) 
264 { 



265 if (a>=b) 

266 return a; 

267 else 



268 return b; 
269} 

270/*ed*/ 

271 

272 

273 

274 

275 

276 

277 



^5^c**:^c:^c:^c:^c:|c*:^c*5^::^::^c5^::^:*3(:5(:^|::^c5^:5lc5^:5^:5^:*H:***************************:^t************♦** 



278 End of math.c 

279 
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APPENDIX H: SOURCE CODE (UTILS.C) 



[ I* * 

2 * * 

3 *FILE: UTILS .C =*= 

4 * * 

5 * ENVIRONMENT: GCC COMPILER V2.7.2 * 

6 * LAST UPDATE: 03 FEBRUARY 1997 * 

7 *NAME: THORSTEN LEONARDY * 

8 * PURPOSE: PROVIDES THE UTILITY FUNCTIONS FOR 

PROGRAM SHEPHERD. * 

9 * * 

10 * COMPILED: >GCC -C -M68040 -O UTILS.O UTILS.C * 

11 * * 

12 * */ 

13 

14 #INCLUDE "SHEPHERD.H" 

15 #INCLUDE "UTILS.H" 

16 #INCLUDE "MATH.H" 

17 

18 

1 9 UNSIGNED INT PIFLAG=0; 

20 UNSIGNED INT MAGIC=0X 1 237; 
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2 1 EXTERN CHAR JO YSTICK[] ; /* DEFINED IN SHEPHERD.C */ 

22 EXTERN CHAR BCDSTRING[]; I* DEFINED IN SHEPHERD.C *! 

23 

24 

25 I* * 

26 * READCLOCKO * 

27 * * 

28 * ENVIRONMENT: GCC COMPILER V2.7.2 

* 

29 * LAST UPDATE: 26 FEBRUARY 1997 * 

30 * NAME: THORSTEN LEONARDY * 

3 1 * PURPOSE: THIS FUNCTION READS THE VALUES FROM THE 
CALENDAR CLOCK * 

32 * DEVICE MTK48T08 (SEE OMNBYTE HANDOUT CHAP. 

2.9.4) INTO * 

33 * GLOBAL VARIABLE CLOCK. THE FORMAT IN CLOCK 

IN DECIMAL IS: * 

34 * * 

35 * CLOCK = YYMMDDHHMMSS * 

36 * * 

37 * I.E TO RETRIEVE THE DATE PERFORM 

D ATE=CLOCK/ 1 000000; * 

38 * TO RETRIEVE THE TIME PERFORM 

TIME=CLOCK% 1000000; * 

39 * * 

40 * CALLED BY; FUNCTION TIMERHANDLER IN TIMER.C 

* 
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41 



42 * 

43 

44 

45 

46 

47 

48 I* 



49 * PITESTO 

50 * 

51 * ENVIRONMENT: 



GCC COMPILER V2.7.2 



52 * LAST UPDATE: 24 FEBRUARY 1997 * 

53 *NAME: THORSTEN LEONARDY * 

54 * PURPOSE: THIS PTJNCTION TESTS INTERPROCESSOR 

SIGANNLING VIA PI-46 * 

55 * INTERRUPT. * 

56 * */ 

57 VOID PITEST(VOID) 

58 { 

59 LONG *VADR; 

60 UNSIGNED CHAR *P; 

61 

62 /* SET ADDRESS FOR PROCESSOR INTERRUPT HANDLER 

ROUTINE */ 
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63 



VADR=(UNSIGNED LONG *)VBA_PI; 



64 *VADR=(UNSIGNED LONG)PfflANDLER; 

65 

66 P=(UNSIGNED CHAR *)ISM_PI; 

67 *P=(UNSIGNED CHAR)0XE0; /* SET IP-ISM TO 68040 ON 
LIRQ-6 */ 

68 

69 P=(UNSIGNED CHAR *)VIC_LIRQ6; 

70 *P=(UNSIGNED CHAR)0X06; /* CONFIGURE VIC068 LIRQ-6 

*1 

71 

72 P=(UNSIGNED CHAR *)APP_ICR; /* ABORT/PROC 

INTERRUPT CTRL */ 

73 *P = *P I (UNSIGNED CHAR)0X02; /* ASSERT IP-46 E4TERRUPT 

*1 

74 

75 WHILE(PIFLAG==0) { INTCOUNTER=0; } /* WAIT FOR PI 

INTERRUPT */ 

76 

77 IF (PIFLAG==MAGIC) { 

78 /* TOGGLEVME((UNSIGNED CHAR *)VME92 10,0X02); */ 

79 SIOOUT(0,"PASSED"); 

80 ) 

81 ELSE { 

82 /* TOGGLEVME((UNSIGNED CHAR *)VME942 1,0X02);*/ 

83 SIOOUT(0,"FAILED"); 
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84 



} 



85 

86 RETURN; 

87 } /* END OF PITEST */ 

88 

89 

90 

91 /* * 

92 * SETVMEO * 

93 * * 

94 * ENVIRONMENT; GCC COMPILER V2.7.2 

* 

95 * LAST UPDATE: 24 FEBRUARY 1997 * 

96 *NAME: THORS TEN LEONARD Y * 

97 * PURPOSE: THIS FUNCTION OUTPUTS DATA TO THE 

STATUS REGISTER OF THE * 

98 * SPECIFIED VME BOARD. * 

99 * * ! 

100 VOID SETVME(UNSIGNED CHAR *BOARD ADDRESS, UNSIGNED 
CHAR DATA) 

101 { 

102 BOARDADDRESS = BOARDADDRESS + 0X81; I* ACCESS 
STATUS REGISTER */ 

103 *BOARDADDRESS=DATA; /* WRITE DATA *! 

104 RETURN; 

105 ) 
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106 



107 

108 

109 /* * 

110 * TOGGLEVMEO * 

111 * * 

112 * ENVIRONMENT: GCC COMPILER V2.7.2 

* 

113 * LAST UPDATE: 24 FEBRUARY 1997 * 

114 *NAME: THORSTEN LEONARDY * 

115 * PURPOSE: THIS FUNCTION PERFORMS AN XOR 

OPERATION ON THE STATUS * 

116 * REGISTER OF THE SPECIFIED VME BOARD. 



117 * *! 

118 VOID TOGGLEVME(UNSIGNED CHAR *BOARD, UNSIGNED 
CHAR DATA) 

119 { 

1 20 BOARD = BOARD + 0X8 1 ; /* ACCESS STATUS REGISTER *! 

121 *BOARD = ABOARD ^ DATA; /* TOGGLE BIT WITH BITWISE 
XOR */ 

122 RETURN; 

123 ) 

124 

125 

126 
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127 /* 



128 * INITBOARDSO 

129 * 

130 * ENVIRONMENT: GCC COMPILER V2.7.2 

131 * LAST UPDATE: 24 FEBRUARY 1 997 



132 *NAME: THORSTEN LEONARDY * 

1 33 * PURPOSE: THIS FUNCTION INITIALIZES ALL VME BOARDS. 

* 

134 =1= */ 



135 VOID INITBOARDS(VOID) 

136 { 

1 37 UNS IGNED CHAR *P; 

138 INTIX; 

139 

140 P=(UNSIGNED CHAR*)VIC_TTR; /* VIC TRANSFER 

TIMEOUT REGISTER */ 

141 *P = 0XFF; /* DISBLE ALL WATCHDOGS */ 

142 

143 P=(UNSIGNED CHAR*)VIC_ICR; I* VIC INTERFACE 

CONFIGURATION REG. */ 

144 *P=0X40; I* PREVENT DEADLOCKS, THIS IS A 

MUST! */ 

145 

146 P=(UNSIGNED CHAR*)VME942 1+0X81; I* ACCESS STATUS 
REGISTER FOR DI *! 
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147 *P = 0X03; /* DISABLE SYSFAIL SIGNAL, SET 

GREEN *! 

148 

149 P=(UNSIGNED CHAR*)VME92 10+0X81; I* ACCESS STATUS 
REGISTER FOR DA */ 

150 *P = 0X03; /* DISABLE SYSFAIL SIGNAL, SET 

GREEN *! 

151 

152 

153 I* * 

154 * INITIALIZE ALL EIGHT QUADRATURE COUNTERS (WHEEL 
ENCODER) * 

155 * *! 

156 

157 P=(UNSIGNEDCHAR*)VMECTR1; 

158 FOR (IX=0; IX<8; EX++) { /* READ MOTORS 

SUBSEQUENTIALLY *! 



159 


*(P+3)=0X20; 


/* CR: MASTER RESET 


*/ 


160 


*(P+3)=0X48; 


!* IC: ENABLE COUNTING 


*! 


161 


*(P+3)=0XC1; 


!* QR: COUNT FULL CYCLE 


*/ 


162 


P=P+0X04; 


!* ACCESS NEXT COUNTER 


*! 


163 IF (EX==3) P=(UNSIGNED CHAR*)VMECTR2; !* ACCESS THE 

SECOND VME COUNTER *! 


164 


} 






165 








166 


SIOOUT(0,"BOARDS INITIALIZED ...\N\R"); 
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167 

168 

169 

170 

171 

172 

173 

174 

175 

176 

177 

178 

179 

180 

181 

182 

183 

184 

185 

186 

187 

188 

189 

190 



RETURN; 



) 

I* MODIFffiD ED MAYS 18 APR 97 *! 
UNSIGNED CHAR B2A(INT VALUE) 
{ 

UNSIGNED CHAR CHAR; 

IF (VALUE < 10) { 

CHAR = 48 + VALUE; 

} 

ELSE { 

CHAR = 55 + VALUE; 



RETURN CHAR; 

} 

VOID B2A2(UNSIGNED CHAR *S, UNSIGNED CHAR CC) 

{ 

INT LOW, HIGH; 

LOW = CC & OXOF; 

HIGH = CC/16; 
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191 



192 *S = B2A(LOW); 

193 *(S-1) = B2A(HIGH); 

194 } 

195 

196 

197 

198 ASSEMBLER ROUTINES 

199 

200 

201 ASM(" 

202 .EVEN 

203 .TEXT 

204 .GLOBE _PIHANDLER 

205 

206 _PIHANDLER; 

207 MOVE.L #0XFFF4800C,A1 /* LOAD APP-ICR INTO A1 

*! 

208 AND.B #0XFD,(A1) I* REMOVE PENDING IP-46 

INTERRUPT SIGNAL *! 

209 MOVE.L #0XI237,_PIFLAG /* SET PIFLAG VARIABLE 

*/ 

210 RTE 



194 



211 



); 

212 

213 

214 /* * 

215 * CONVERTTOASCnO * 

216 * * 

217 * ENVIRONMENT: GCC COMPILER V2.7.2 * 

218 * LAST UPDATE: 02 MAY 1997 * 

219 *NAME: THORSTEN LEONARD Y * 

220 * PURPOSE: THIS FUNCTION CONVERTS AN UNSIGNED 

INTEGER TO ITS ASCU * 

221 * EQUIVALENT AND WRITES THIS EvITO A STRING. 

* 

222 * NDIGITS NUMBER OF DIGITS TO CONVERT 

* 

223 *DATA THE INTEGER TO CONVERT * 

224 *STR POINTER TO STRING * 

225 * */ 

226 VOID CONVERTTOASCn(UNSIGNED INT NDIGITS, UNSIGNED 
INT DATA, CHAR *STR) 

227 { 

228 UNSIGNED INT I; 

229 

230 STR=STR+NDIGITS-1; 

231 FOR (I=0;I<NDIGITS;I++) { 
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232 *STR- = 'O' + DATA % 10 ; 

233 DATA=DATA/10; 

234 } 

235 RETURN; 

236 } 

237 

238 

239 I* * 

240 * READJOYSTICKO * 

241 * * 

242 * ENVIRONMENT: GCC COMPILER V2.7.2 

* 

243 * LAST UPDATE: 02 MAY 1997 * 

244 *NAME: THORSTEN LEONARDY * 

245 * PURPOSE: THIS FUNCTION READS THE THREE PORTS (A,B 

AND C) FROM THE * 

246 * INTEL 85C55 PARALLEL PORT 1 AND CONVERTS 

THEM INTO AN ASCII * 

247 * STRING. * 

248 * *! 

249 

250 VOID READJOYSTICK(VOID) 

251 { 

252 UNSIGNED INT I,INDEX; 

253 UNSIGNED CHAR *CTRLPORT=(UNSIGNED CHAR*)PI01_CTRL; 
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UNSIGNED 



CHAR 



*DATAPORT=(UNSIGNED 



254 

CHAR*)PI01_DATA; 

255 UNSIGNED INT PIOPORTl [3]; 

256 DOUBLE A= 0.1, XX, YY, ZZ; 

257 

258 *CTRLPORT=OX9B; /* SET ALL PORTS (A,B,C) INTO INPUT 
MODE (READ ONLY) *! 

259 INDEX=10; I* POSITION FOR X-DIGITS E4 STRING 

JOYSTICK */ 

260 

261 FOR (I=0;I<3;I++) 

262 PIOPORT 1 [I] = *(DATAPORT+I); 

263 

264 XX = (DOUBLE)PIOPORT 1 [0]- 128.0; 

265 YY = (DOUBLE)PIOPORT1[1]-128.0; 

266 IF (XX >= 0.0) 

267 XX = XX*XX/100; 

268 ELSE 

269 XX = -XX*XX/100; 

270 IF (YY >= 0.0) 

271 YY= YY*YY/100; 

272 ELSE 

273 YY = -YY*YY/100; 

274 JOYSTICK.X = A*(XX) + ( 1 .0-A)*JOYST1CK.X; 

275 JOYSTICK. Y = A*( YY) + ( 1 .0-A)* JOYSTICK. Y; 



197 



276 



277 IF (PIOPORTl [2]==0X03) 

278 SETVME((UNSIGNED CHAR *)VME9210,0X00); I* NO BUTTON 
PRESSED */ 

279 ELSE { 

280 SETVME((UNSIGNED CHAR *)VME92 10,0X02); /* IF ANY 
BUTTON PRESSED *! 

281 } 

282 } 

283 

284 

285 DOUBLE INSENSmVE(DOUBLE Z) 

286 { 

287 IF(Z>=10.0) 

288 RETURN (Z- 10.0); 

289 ELSE 

290 IF(Z<=-10.0) 

291 RETURN (Z+ 10.0); 

292 ELSE 

293 RETURN 0.0; 

294 } 

295 

296 VOID DISPLAYJOYSTICKO 

297 { 

298 CONVERTINT(BCDSTRING+9, (INT)JOYSTICK.X); 
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299 BCDSTRING[3]='0'; 

300 BCDSTRING[4]='3'; 

301 BCDSTRING[6]='4'; 

302 BCDSTRING[7]='0'; 

303 SIOOUT(0,BCDSTRING); 

304 

305 CONVERTINT(BCDSTRING+9, (INT)JOYSTICK.OMEGA); 

306 BCDSTRING[3]='0'; 

307 BCDSTRING[4]='4'; 

308 BCDSTRING[6]='4'; 

309 BCDSTRING[7]='0'; 

310 SIOOUT(0,BCDSTRING); /* OUTPUT UPDATED POSITION 
STRING TO SCREEN *! 

311 RETURN; 

312 } 

313 

314 

315 END OF UTILS. C 

316 

sje * y 
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APPENDIX I: SOURCE CODE (SERIAL.C) 



1 /* * 

2 * * 

3 * FILE: SERIAL.C * 

4 * * 

5 * ENVIRONMENT: GCC COMPILER V2.7.2 * 

6 * LAST UPDATE: 26 FEBRUARY 1997 * 

7 * NAME: THORSTEN LEONARDY * 

8 * PURPOSE: PROVIDES ROUTINES FOR SERIAL INPUT 

AND OUTPUT TO THE 68C681 * 

9 * ON THE TAURUS BOARD. * 

IQ * * 

1 1 * COMPILED: >GCC -C -M68040 -O SERIAL.O SERIAL.C* 

12 * * 

13 * */ 

14 

15 #INCLUDE "SHEPHERD.H" 

16 #INCLUDE "SER1AL.H" 

17 

18 /* * 

19 * GLOBAL VARIABLES * 



21 
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22 /* UNSIGNED INT COUNTER; /* COUNT THE INTERRUPTS */ 

23 UNSIGNED CHAR INPORTA; /* CHARACTER READ FROM SERIAL 
PORT */ 

24 

25 /*VT100 CONTROL SEQUENCES*/ 

26 

27 /* POSITION CURSOR, CUP = ESC [ 'O' 'O' ; 'O' 'O' H */ 

28 UNSIGNED CHAR VT100XY[9]={27, 91, 48,48,59,48,48,72,0}; /* POSITION 

CURSOR */ 

29 

30 /* ERASE IN DISPLAY ED TO CLEAR THE SCREEN */ 

31 UNSIGNED CHAR CLRSCR[5]={27,91,50,74,0}; /* ESC [ '2' J */ 

32 

33 /* ESC-SEQUENCE EL (ERASE IN UNE) TO ERASE A LINE */ 

34 UNSIGNED CHAR CLRLINE[6]= {5,27,91,50,75,0}; /* ESC [ '2' K */ 

35 

36 /* ESC-SEQUENCE PRINT SCREEN (ESC [ I) */ 

37 UNSIGNED CHAR PRTSCR[4]= {27,91,105,0}; /* ESC [ I */ 

38 

39 

40 /* ESC-SEQUENCE SGR (SELECT GRAFIK RENDITION) (ESC [ 0 M ) */ 

41 UNSIGNED CHAR CURSOROFF[5]= {27,91,0,109,0}; /♦ CURSOR BLINK 

OFF*/ 

42 

43 /♦ * 
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44 

45 

46 

47 

48 

49 

50 

51 

52 

53 

54 

55 

56 

57 

58 

59 

60 

61 

62 

63 

64 

65 

66 



* SIOOUTO * 

* ENVIRONMENT: GCC COMPILER V2.7.2 * 

* LAST UPDATE: 07 JANUARY 1 997 * 

* NAME: THORSTEN LEONARDY * 

* PURPOSE: THIS FUNCTION OUTPUTS A STRING 

TO ONE OF THE TWO SERIAL * 

* PORTS. * 

* * 

* HOSTFLAG 0 -> OUTPUTS TO CONSOLE (PORT A) * 

* 1 -> OUTPUTS TO HOST (PORT B) * 



^ * 

* S POINTER TO THE OUTPUT STRING * 

* * 

* 



VOID SIOOUT(INT HOSTFLAG, UNSIGNED CHAR *S) 

{ 

UNSIGNED CHAR *P=(UNSIGNED CHAR *)CONSOLE; 

IF (HOSTFLAG) P+=8; /* ACCESS HOST REGISTERS */ 

/* OTHERWISE ACCESS CONSOLE */ 

WH1LE(*S) { 

WHILE ((*(P+1)&4)==0); /* SRA: WAIT UNTIL TX READY V 
*(P+3)=*S++; /* OUTPUT CHARACTER */ 
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67 



} 



68 

69 RETURN; 

70 } /* END OF SIOOUT V 

71 

72 

73 

74 /* 

75 * GOTOXYO 

76 * ENVIRONMENT: GCC COMPILER V2.7.2 

77 * LAST UPDATE: 14 FEBRUARY 1997 



78 * NAME: THORSTEN LEONARDY * 

79 * PURPOSE: THIS FUNCTION POSITIONS THE CURSOR ON THE 

SCREEN. * 

80 * * 

81 *X ROW FOR CURSOR POSITION (X=0..20) * 

82 *Y COLUMN FOR CURSOR POSITION (Y=L. 80) * 

83 * * 

84 * */ 

85 



86 VOID GOTOXY(INT X, INT Y) 

87 { 

88 

89 IF ((X>0)&(X<81)&(Y>0)&(Y<33)) { 
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90 

91 

92 

93 

94 

95 

96 

97 

98 

99 

100 

101 

102 

103 

104 

105 

106 

107 

108 

109 

no 



VTl 00XY[2] =48+X/l 0; 

VT100XY[3]=48+X%10; 

VTl 00XY[5] =48+ Y/ 1 0; 

VTl 00XY[6] =48+Y%l 0; 

SIOOUT(0,VT1 OOXY); /* OUTPUT ESCAPE -SEQUENCE */ 

} 

RETURN; 

} 

* 

* SIOINITO * 

* ENVIRONMENT: GCC COMPILER V2.7.2 * 

* LAST UPDATE: 26 FEBRUARY 1997 * 

* NAME: THORSTEN LEONARDY * 

* PURPOSE: THIS FUNCTION INITIALIZES BOTH 

SERIAL PORTS. IN ADDITION, * 

* PORT A (CONSLE)IS INITIALIZED FOR 

INTERRUPT DRIVEN I/O * 

* * j 



VOID SIOINIT(\'OID) 

{ 
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1 1 1 UNSIGNED CHAR *P=(UNSIGNED CHAR*)CONSOLE;/* BASE ADDRESS 
FOR 68C681 DUART */ 

112 LONG*VADR; /* FOR VBA REGISTER ENTRY V 



113 

114 

115 /* */ 

116 /* INITIAUZE CONSOLE (PORT A) */ 

117 /* */ 

118 /* ATTENTION: THESE SETTINGS HAVE TO AGREE WITH THE 
SETTINGS FOR */ 

119 /* YOUR TERJVONAL (I.E. LAPTOP COMPUTER) */ 



120 *(P+2)=(UNSIGNED CHAR)0X2A; /* CRA: RESET RX,DISABLE RX & 
TX */ 

121 *(P+2)=(UNSIGNED CHAR)0X1A; /* CRA: RESET MR POINTER, 

V 

122 *(P+0)=(UNSIGNED CHAR)0X13; /* MRIA: RX CONTROLS RTS, 

V 

123 /* 8 BITS, NO PARITY */ 

124 *(P+0)=(UNSIGNED CHAR)0X07; /* MR2A: NORMAL MODE, 1 STOP 
BIT */ 

125 *(P+1)=(UNSIGNED CHAR)0XBB; /* SET BAUD RATE 9600 BAUD 

V 

126 *(P+2)=(UNSIGNED CHAR)0X15; /* ENABLE RX AND TX 

V 

127 

128 

129 /* */ 

1 30 /* INITIALIZE HOST (PORT B) */ 
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131 


/* V 


132 

V 


*(P+10)=(UNSIGNED CHAR)0X1A; /* CRB: RESET MR POINTER 


133 

V 


*(P+8)=(UNSIGNED CHAR)0X13; /* MRIB: NO PARITY, 8 BITS 


134 

BIT */ 


*(P+8)=(UNSIGNED CHAR)0X07; /* MR2B: NORMAL MODE, 1 STOP 


135 

V 


*(P+9)=(UNSIGNED CHAR)0XBB; /* SET BAUD RATE 9600 BAUD 


136 

V 


*(P+ 10)= (UNSIGNED CHAR)0X15; /* CRB: ENABLE RX AND TX 



137 



138 




139 




140 

V 


/* IT FOLLOWS THE INTERRUPT SPECIFIC PART FOR PORT A 


141 


y/* * j 



142 *(P+5)=(TJNSIGNED CHAR)0X02; /* ISR: SET INTERRUPT MASK FOR 
RXRDYA V 



143 


*(P+12)=0X60; /* IVR: PLACE INTERRUPT VECTOR */ 


144 


/* 0X60 ACCESSES VBA AT BASE+OXl 80 */ 


145 

V 


VADR=(LONG*)0XFFE401 80; /* VBA ADDRESS FOR INTHANDLER 


146 

V 


*VADR=(LONG)INPORTAHANDLER; /* WRITE ADDRESS INTO VBR 


147 





148 P=(UNS1GNED CHAR*)ISM_SERIAL; /* ISM CONFIGURATION 
REGISTER FOR SIO */ 

149 *P=0X09; /* INTERRUPTS TO 68040 ON LIRQ-1 ♦/ 
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150 



1 51 P=(UNSIGNED CHAR*)VIC_LIRQ1; /* VIC068 LICR FOR LIRQ-1 FROM 
ISM */ 



/* ASSERT IRQ-1 FROM VIC TO 68040 */ 



152 *P=0X01; 

153 

154 

155 RETURN; 

156 } 

157 

158 

159 

160 

162 ASSEMBLER ROUTINES 

164 

165 /* * 

166 * INPORTAHANDLERO * 

167 * * 

168 * ENVIRONMENT: GCC COMPILER V2.7.2 * 

169 * LAST UPDATE: 27 JANUARY 1997 * 

170 *NAME: THORSTEN LEONARDY * 

171 * PURPOSE: INTERRUPT HANDUNG ROUTINE FOR INTERRUPTS 

FROM 68C681 DUART * 
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1 72 * IT READS A OiARACTER INPUT FROM THE KEYBOARD INTO 
VARIABLE * 

173 * INPORTA, INCREMENTS A COUNTER, AND OUTPUTS THE 

CHARACTER * 

174 * TO THE SCREEN. IF A CR IS TYPED AT THE KEYBOARD, AN 

* 

175 * ADDITIONAL LINEFEED (OXOA) IS ADDED TO THE <CR> 

(OXOD). * 

176 * * 

177 * */ 



178 

179 ASM(" 

180 

181 .EVEN 

182 .TEXT 

183 .GLOBL_INPORTAHANDLER 

184 

185 _INPORTAHANDLER: 

186 

187 LINK A6,#-128 /* ALLOCATE 184 BYTES ON STACK TO ... V 

188 FSAVE A6@(-128) 

189 MOVEML D0-D7/A0-A5,SP@- /* SAVE REGISTERS (14*4 B\nTE) 

190 

191 MO\^.L #OXFFF4AOOOA2 /* BASE ADDRESS OF 68C681 DUART 

V 

192 MOVE.B 3(A2),D2 /* RHR_A: READ CHARACTER ... */ 
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193 

194 

195 

196 

197 

198 

199 

200 

201 

202 

203 

204 

205 

206 

207 



MOVE.B D2^INPORTA 



/* ... AND COPY TO INPORTA 



*/ 



MOVEML SP@+,D0-D7/A0-A5 
FRESTORE A6@(-128) 

UNLK A6 



RTE 









END OF SERIAL.C 






/ 
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APPENDIX J: SOURCE CODE (CONSOLIDATED HEADER FILES) 
The following code was modified by: Professor Kanayama, Thorsten Leonardy, 
Edward Mays, and Ferdinand A. Reid. 

1 /* shepherd.h */ 

2 

3 #ifndef SHEPHERD.H 

4 #define SHEPHERD.H 

5 

6 I* * 

7 * Base Addressees for accessing Servo Control Cards * 

8 * */ 

9 



10 #define VME9210 


0xffff0400 


/* Base Address analog out to servo 


*/ 


1 1 #define VME9421 


OxffffOOOO 


/* Base address data in from servo 


*/ 


12 #define VME2170 


OxffffffOO 


/* Base address data out to servo 


*/ 



13 #define VMECTRl OxffffbOOO /* VME Counter for driving motor */ 

14 #define VMECTR2 OxffffblOO /* VME Counter for steering motor */ 

15 

16 

17/* * 

18 * defines for general Interrupt Handling * 
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19 



* 



*/ 



20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 



#define VIC.LIRQl 0xfff44027 
#define VIC_LIRQ2 0xfff4402b 
#define VIC_LIRQ3 0xfff4402f 
#define VIC_LIRQ4 0xfff44023 
#define VIC_LIRQ5 0xfff44037 
#define VIC_LIRQ6 0xfff4403b 
#define VIC_LIRQ7 0xfff4403f 

#define VIC TTR 0xfff44043 



/* VIC068 Register for LIRQ-1 */ 
/* VIC068 Register for LIRQ-2 */ 
/* VIC068 Register for LIRQ-3 */ 
/* VIC068 Register for LIRQ-4 */ 
/* VIC068 Register for LIRQ-5 */ 
/* VIC068 Register for LIRQ-6 */ 
/* VIC068 Register for LIRQ-7 */ 



/* Transfer Timeout Register */ 
/* see p. 4-2 TAURUS Manual */ 

#define VIC_ICR 0xfff440af /* VIC Interface Configuration */ 



#define enable() asm("move.w #0x2000,sr") I* enable interrupts */ 
#define disable!) asm("move.w #0x2700,sr") /* disable interrupts */ 



/* defines for Vector base register entries */ 

#define VBA_TEMER 0xffe40l30 /* Vector table address for Timer-5 ISR 
#define VBA_PI 0xffe401 18 /* Vector table entry for IP interrupt */ 



*/ 
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41 



42 

43 /* 



44 * defines for interrupt steering mechanism * 

45 * *! 

46 

47 #define ISM_TIMER 0xfff48004 /* ISM Configuration Register for Timer A */ 

48 #define ISM_PI 0xfff48008 /* ISM Configuration Register for PI */ 

49 #define ISM_SERIAL 0xfff48001 /* ISM Configuration Register for serial lO */ 

50 

5 1 #define APP_ICR 0xfff4800c /* abort/processor interrupt control register */ 

52 

53 

54 

55 I* * 



56 * Base Addressees for accessing Parallel lO-Ports * 

57 * *! 



58 #definePI01 CTRL 0xfff40003 



59 #define PIOl DATA 0xfff40000 



60 #define PI02 CTRL 0xfff40007 



61 #defme P102 DATA 0xfff40004 



/* control register for PlO-1 */ 

/* data register for PIO-1 Port A */ 
/* control register for PIO-2 *! 

I* data register for PlO-2 Port A */ 



62 
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63 



64 /* * 

65 * Base Addressees for 68030 Input/Output Program * 

66 * as outlined in Taurus Manual, Chapter 6 * 

67 * *! 

68 #define IOP_CMDBLK OxffeOOOOO /* address for lOP Command Block *! 

69 #define IOP_START 0x01 /* command to start lOP */ 

70 #define IOP_STOP 0x00 /* command to stop lOP */ 

1 1 #define IOP_COMPLETE 0x80 /* mask for operation complete */ 

72 

73 #define IOPB_CONFIGURE OxeO /* command to configure lOBP */ 

74 #define IOPB_UNIT_OMNIO 0x10 /* unit # for omnimodule #0 */ 

75 I* * 

76 * definitions for 68030 Input/Output Program, (Leo, 05/13/97) * 

77 * *! 

78 

79 

80 /* Input/Output Parameter Block structure, according Taurus Manual, p. 6-4 */ 

8 1 typedef struct { 

82 unsigned char cmd; /* command */ 

83 unsigned char error; /* error status */ 

84 unsigned short options; /* options */ 

214 



85 unsigned short reserved; /* reserved, do not use */ 

86 unsigned char unit; /* unit number */ 

87 unsigned char destUnit; /* destination unit */ 

88 unsigned long blockNumber;/* logical Block number */ 

89 unsigned long txCount; /* Transfer count, # of bytes to transfer *! 

90 unsigned long *ptrSrc; /* address of source */ 

91 unsigned long *ptrDst; /* Address of destination */ 

92 }IOPB; 

93 

94 /* Command Block structure according to Taurus Manual, p. 6-3 */ 

95 typedef struct { 

96 unsigned char cmd; /* status and command register */ 

97 unsigned char reserved[3]; /* not yet used */ 

98 lOPB *ptrToIOPB; /* pointer to lOBP */ 

99 }CMD_BLOCK; 

100 
101 

102/* Omnimodule support block structure according to Taurus Manual p. 6-12 */ 
103typedef struct { 

104 unsigned long options; /* 4 bytes options, unused */ 

105 unsigned long *ptrlnit; /* pointer to initialization routine */ 

106 unsigned long *ptrTask; /* pointer to task */ 
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107 unsigned long *ptrlntr; I* pointer to interrupt servicing routine *! 

108)OSB; 

109 

1 lOIOPB iopbOMNIO; /* lOBP for Omnimodule 0 (used for serial I/O to VTIOO */ 
1 1 1 OSB osbOMNIO; /* OSB for Omnimodule 0 (used for serial I/O to VTIOO */ 



112 

113/* */ 

114/* */ 

115 



1 1 6unsigned int intCounter, testCounter; I* count the interrupts */ 

1 17 unsigned int demo; /* switch to run demo see driver() in movements */ 

1 ISunsigned short timer_in_ms; /* desired timer period in ms */ 

119 

120 

121 /* * 

122 * definitions for inertial measurement routines (imu.c) * 

123 * */ 

124 

125/* added 10 Sep 97 */ 

126typedef struct { 

127 unsigned short ax; /* linear acceleration in x-direction */ 

128 unsigned short ay; /* linear acceleration in y-direction */ 
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129 unsigned short az; /* linear acceleration in z-direction */ 

130 unsigned short omega_z; /* angular velocity in z-direction */ 



131}IMU; 

132 

133IMU imu; /* stores most recent IMU data (updated with */ 

134 /* every 10ms timer interrupt */ 

135 

136 /=^ * 



137 * definitions for Joystick Control, (Leo, 05/10/97) 



138 *! 

139 

MOtypedef struct { 

141 double x; /* x position (or velocity) */ 

142 double y; /* y position (or velocity) */ 

143 double omega; /* angular velocity */ 



144 unsigned char state; /* status of parallel port 1, channel C */ 
145)JPOINT; 

146 JPOINT joystick; /^global*/ 

147 

148typedef struct { 

149 double x; 

150 double y; 
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151 }point; 

152 

153 

1 54typedef struct { 

155 point coord; 

156 double heading; 

1 57 double kappa; 

158 } Configuration; 

159 Configuration vehicle; /^global*/ 

160 

161typedef struct { 

1 62 double Speed; 

1 63 double Theta; 

164 double Omega; 

1 65 } vehicleMotion; 

166 vehicleMotion motion, motionO; /*global*/ 

167 

1 68typedef struct { 

1 69 double rho; 

170 double alpha; 

171 } polar; 

172 
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173 



174/* */ 

175/* definitions for wheel control */ 

176/* */ 

177 

178/* write these masks to VME2710 at address OxffffffOO in order to make */ 
179/* the specific motor drive! May wish to logical OR with previous settings */ 
180 



181#define TURN.FR 


0x00004000 


182#define TURN_FL 


0x00020000 


183#define TURN.RR 


0x00100000 


184#define TURN.RL 


0x00800000 


185 




186#define DRIVE_FR 


0x00000004 


187#define DRIVE_FL 


0x00000020 


188#define DRIVE_RR 


0x00000100 


189#define DRIVE_RL 


0x00000800 


190 





/* turn wheel 1 (front right) */ 
/* turn wheel 2 (front left) */ 

/* turn wheel 3 (rear right) */ 
/* turn wheel 4 (rear left) */ 

/* drive wheel 1 (front right) */ 
/* drive wheel 2 (front left) */ 
/* drive wheel 3 (rear right) */ 
/* drive wheel 4 (rear left) */ 



191#define ALL_WHEELS 0x00924924 /* select all wheels for turning */ 



192 /* and driving */ 

193 

194/* * 
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195 * function definitions * 



196 * */ 

197 

198void setVME(unsigned char *board, unsigned char data); 
199void toggleVME(unsigned char *board, unsigned char data); 
200void initBoards(void); 

201 void piTest(void); 

202void piHandler(void); 

203 void advanceCountO; 

204 /* global variable to make joystock coordinates accessible */ 

205 

206#define ARRAY.SIZE 4 
207#define DegreesToRads 0.0174532925 
208#define RadsToDegrees 57.29577951308232 
209#define DeltaT 0.01 
210 
211 

2 1 2double desiredAngleRates[ARRAY_SIZE], 

213 desiredAngleRatesO[ARRAY_SIZE], 

214 desiredSpeeds_F[ARRAY_SIZE], 

2 1 5 desiredAngleRates_F[ ARRAY_S1ZE], 

216 desiredSpeeds[ARRAY_SlZE], 
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217 



actualSpeeds[ARRAY_SlZE], /* 28 May ejm */ 
actual An gleRates [ARRAY_S IZE] , 



218 

219 DigitToCmDrive[ARRAY_SIZE], 

220 Display_Speeds[ARRAY_SlZE], 

221 Display_Steers[ARRAY_SIZE], 

222 

223 desiredAngles[ARRAY_SIZE], 

224 desiredAnglesO[ARRAY_SlZE], 

225 actualAngles[ARRAY_SIZE]; 

226 

227short Steer_Digits[ARRAY_SI2E], 

228 Speed_Digits[ARRAY_SlZE]; 

229 

230double WheelDriveAct[ARRAY_SIZE], 

231 WheelDriveDes[ARRAY_SIZE]; 

232 

233unsigned long int WheelDriveActO[ARRAY_SIZE], 

234 WheelDriveActl[ARRAY_SlZE], 

235 driveReadings[ARRAY_SIZE]; 

236 

237double WheelDirAct(ARRAY_SIZE], 

238 WheelDirDes[ARRAY_SIZE], 
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239 



PreviousCountSpeed[ARRAY_SIZE], 



240 PreviousCountSteer[ARRAY_SIZE]; 

241 

242unsigned long int WheelDirActO[ARRAY_SIZE], 

243 WheelDir Act 1 [ ARRAY_SIZE] , 

244 steerReadings[ARRAY_SIZE]; 

245 

246int mode, 

247 oldMode, 

248 modeOstate, 

249 modeSstate, 

250 modeTstate, 

251 Flag, 

252 oldFlag, 

253 edCounter, 

254 hallSensor3; 

255unsigned int intCounter, testCounter; /* count the interrupts */ 

256 

257 

258 

259/*unsigned long int *! 

260double previousCount,previousCountSteer, Omega_Speed, 
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261previousCountSpeed;/*previousCount represents infinity */ 

262 

263double K1[ARRAY_SIZE], 

264 K2[ARRAY_SIZE], 

265 K3[ARRAY_SIZE], 

266 K4[ARRAY_SIZE], 

267 K6[ARRAY_SIZE]; /* slope based on input units vs output velocity, */ 

268 /* input range from 0- 1020, feedback constant */ 

269 /* K3 is the inverse of (86.975velocity/1020 digit) */ 

270 

271 

272#endif 

273 

275 End of shepherd. h 

277 

278 

279 #ifndef _MOVEMENT_H_ 

280#define _MOVEMENT_H_ 

281 

282#include "shepherd. h" 
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283 



284 



285#define PI 


3.14159265358979323846 




286#define DPI 


6.28318530717958647692 ; 


t* PI*2 


287#define HPI 


1.570796327 


!* PI/2 


*! 


288#define QPI 


0.785398163 


!* PI/4 


*/ 


289#define QPIbySOO 


0.0015707963 







290 /* QPI/(5 seconds/deltaT) */ 

291 

292 

293 

294 

295 double wheel_speed[4], wheel_angle[4]; 
296 

297 void initMovementO; 

298 void setupPolar(polar []); 

299void wheelMotionO; 

300void bodyMotionO; 

301 void driverO; 

302void joystickMotionlnterface(void); 

303 

304 
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305extem double desired AngleRates[], 

306 desiredSpeeds[], 

307 PreviousCountSpeed[], 

308 PreviousCountSpeed[], 

309 PreviousCountSteer[]; 

310 

31 Ipolar whp[4]; 

3 12double pathLength,thetaDot,omegaDot,speedDot; 

313 

314 

315 

^ j 6/^ for tsn^ditisl motion ^ j 

317 

318 

319double sigma; 

320double radius; 

321 

322double ai[4], bi[4]; 

323typedef struct { 

324 Configuration config; 

325 point center; 

326 double radius; 
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327 double 



a; 



328 double b; 

329 double c; 

330} LINE; 

331 

332static LINE currentPath; /* holds the current path element values */ 
333Configuration incrementalMotion, holdVehicle; 

334static double deltaS; 

335 

336void tangentialMotionO; 

337 void circular Arc(double length, double alpha); 

338void defineConfig(double x, double y, double theta, double kappa); 
339void compose!); 

340double steer(); 

34 1 void constants!); 

342double Psi!point pi, point p2); 

343double distance!point pi, point p2); 

344void initTangentO; 

345 

346#endif 

347 

348 



226 



349 



350 

35 1 #ifndef _MOTOR_H_ 

352#define _MOTOR_H_ 

353 

354#include "shepherd.h" 

355 

356/=^ * 

357 * Base Addressees for accessing Servo Control Cards * 

358 * Used in Home Testing 

359 * *! 

360#define SteerDriveInteract .02 /* used to give stability to wheel */ 
361 

362#define RadRateTodigit 195.3789 /* digit/radpersec*/ 

363 

364#define digitToRadDrive -6.015495746e-5 

365 /* driving constant rad/count = DPI/ 104450 May 8 */ 

366 /* Experimental Results by Ed Mays May 7 */ 

367 /* Wheel 1 count = 104456 *! 

368 /* Wheel 2 count = 104435 *! 

369 I* Wheel 3 count = 104454 *! 

370 /* Wheel 4 count = 104455 */ 
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371 


!* Average count = 104450 


*! 


372 


/* cf. 2048 * 51 = 104448 


*/ 



373#define digitToCmDrive 0.001 1369287 

374 /* driving constant cm/count = digitToRadDrive* 18.9cm 5/8/97 */ 

375 

376#define digitToRadSteer -6.8 1769239 le-5 

377 /* steering constant rad/count = DPI/(2048*45) 19 Apr */ 

378 

379#define SteerFBGain 0.000; /* steering feedback gain */ 

380#define DriveFBGain 0.000; /* driving feedback gain */ 

381#define DigitsHigh 1023 
382#define DigitsLow -1024 

383#define WheelRadius 18.9 /* prev def in cm *! 

384#define VME9210 0xffff0400 /* Base Address analog out to servo */ 

385#define VME9421 OxffffOOOO /* Base address data in from servo */ 

386#define VME2170 OxffffffOO /* Base address data out to servo */ 

387#define VMECTRl OxffffOOOO /* Counter */ 

388#define K5 87.4 /* control feedback constant (cm/sec) variable 28 May ejm */ 

389#define DriveFeedBackGain 0.8 /*.8 control drive feedback gain 28 May ejm */ 

390#define angularK3 0.96963 /* digit/rotational speed (rad/sec) */ 

391#define steerFeedbackGain 100.0 /* steering Feedback gain */ 

392#define angularK5 5.23598 
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393#define angleFeedbackGain 1000.0 



394 

395extem unsigned char clrLine[6]; /* ESC-Sequence for clear line */ 
396extem char bwheeldrivecdString[];/* defined in shepherd.c */ 

397extem unsigned char bcdString[]; 

398 

399double Drive_Feedback[ARRAY_SIZE] ; 

400 

401 extern double desiredAngleRates[], 

402 desiredSpeeds[], 

403 PreviousCountSpeed[], 

404 PreviousCountSteer [], 

405 DigitToCmDrive[], 

406 Display_Speeds[], 

407 Display_Steers[], 

408 desiredAngles[], 

409 actualAngIes[]; 

410 

411/* * 

412 * function definitions * 

413 * *! 

414void driveSpeed(short []); 
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415void driveSteer(short []); 

416void driveMotorsQ; 

417void wheelDrive(void); 

418void allStop(void); 

419void updateEncoders(void); 

420void update WheelDrive(void); 

421 void update WheelSteer(void); 

422 void displayDirections(void); 

423 void displaySpeed(void); 

424int readSteerEncoders(unsigned long int []); 

425void testDrive(void); 

426void readEncoders(void); 

427 void accumulatedriveSpeedO; 

428 void displayDriveAngleO; /* added 15 may */ 

429void drivingFeedbackO; /* 28 May ejm */ 

430double velocityReferenceTable(double,int); /* 28 May ejm */ 
431 void steeringFeedbackO; /* 4 June */ 

432double rateReferenceTable(double); /* 4 June */ 

433void computeActualRatesO; /* 5 June */ 

434int convertDifference(int); /* 1 1 June ejm */ 

435 

436void alignWheels(void); 
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437void clearShaftEncoder(unsigned short motors); 



438void readWheelStatus(unsigned char *array); 
439 

440#endif 

441 

442 

443 



444/* Timer.h */ 

445 

446#ifndef TIMER.H 
447#define TIMER.H 
448 



449/* Defines for Timer control */ 

450#defme TIMER.CTRL 0xfff41002 /* Control register for Timer A */ 

45 1 #define TIMER.DATA 0xfff41000 /* Data register for Timer A */ 

452 

453 

454/* settings master mode register according to fig. 1-12 */ 

455#define TIMER.MASTER.MODE OxbafO /* timer master mode register */ 

456 /* b=BCD count, 16 Bit data bus */ 

457 /* 4=divide by 4 */ 

458 /* 8 = Source F4 (divide by 1000) */ 
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459 



/* 0 = don’t care 



*/ 

460 

461 

A62I* settings for counter mode register according to fig. 1-17 */ 

463#define TIMER_MODE 0x0f3 1 /* Counter Mode Register Bit Assignment */ 

464 /* 0 = no gating, count on rising edge */ 

465 /* 8 = Source F4 (divide f by 1000) */ 

466 I* 3 = BCD repetitive count, reload load */ 

467 /* 2 = count down, toggle TC */ 

468 /* or 1 = count down, active high Terminal Count Pulse */ 

469 

470/* * 

471 * function definitions * 

472 * */ 

473 

474 void TimerHandler(void); 

475 void timerStart(void); 

476 
All 
478 

479#endif 

480 
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481 



482 End of timer.h 

483 

484 

485 

486 

487 

488#ifndef _M ATH_H_ 
489#define _MATH_H_ 

490 

491 

492 

493double cos(double x); 

494double sin(double x); 

495 double atan2(double x, double y); 
496double atan(double x); 

497double new_sqrt(double x); 
498double new_abs(double x); 
499double norm(double angle); 
500double min (double, double); 
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501 double max (double, double); 
502#endif 

503 

504 

505/* 

506 * 

507 * File: S E R I A L . H 

508 * 

509 * Environment: GCC Compiler v2.7.2 

510 * Last update: 13 March 1997 



511* Name: Thorsten Leonardy * 

512 * Purpose: Header File for 'serial.c' * 

513 * */ 

514 



5 1 5#ifndef _SERIAL_H_ 

516#define _SERIAL_H_ 

517 

518 

5 1 9#define CONSOLE 0xfff4a000 /* Base address 68C68 1 DU ART */ 
520 

521/* * 

522 * function definitions * 
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523 * *! 

524 

525 void inPortAHandler(void); /* interrupt handler */ 

526 void siolnit(void); /* initialize DUART *! 

527 void sioOut(int hostHag, unsigned char *s); /* Output a string */ 

528 void gotoXY(int x, int y); /* position cursor */ 

529 

530#endif 

531 

533 End of serial. h 

535 

536 

537/* * 

538 * * 

539 * File; UTILS .H * 

540 * * 

541 * Environment: GCC Compiler v2.7. 2 * 

542 * Last update; 13 March 1997 * 

543 * Name: Thorsten Leonardy * 

544 * Purpose; Header File for 'utils.c' * 
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545 * 



*1 
546 

547#ifndef _UTILS_H_ 

548#define _UXn^_H_ 

549 

550/* * 

551 * function definitions * 

552 * */ 

553 

554 void setVME(unsigned char *board, unsigned char data); 

555 void toggleVME(unsigned char *board, unsigned char data); 

556void initBoards(void); 

557 void piTest(void); 

558void piHandier(void); 

559/* void readClock(void); */ 

560/*void WRITE_CLOCK(void); */ 

561 

562/* Modified 18 Apr */ 

563unsigned char b2a(int); 

564 void b2a2(unsigned char *, unsigned char); 

565void convertToASCII(unsigned int ndigits, unsigned int data, char *str); 
566void readJoyStick(void); 
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567double insensitive(double z); 
568 

569#endif 

570 

572 End of utils. h 
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APPENDIX K: SHEPHERD OPERATING MANUAL 



OVERVIEW 

The Purpose of this document is to provide a quick guide for doing downloads for testing 
or other purposes. For a more detailed guide see the Shepherd Operators Guide (SOG). 

The Shepherd compilation and download process is a four step process: 

- Compile executable on workstation. 

- FTP S-Records to laptop. 

- Use Windows 95 HyperTerminal program for direct connection. 

- Run the program once download complete. 



Compile Executable on Workstation 

1 . Once you have logged in on the Shepherd account, then use the xinit command to 
generate the X-Windows environment. 

2. In the large terminal window type "cap" at the UNIX prompt and press the return key. 

3. The alias "cap" logs you onto capella (the standard login script will scroll by). The 
Shepherd group uses capella (server) because of the nature of the cross compilation used 
for the "Taurus board" and Motorola 68040 CPU. 

4. Next, in the large terminal window type "taurus" at the prompt and press the return 
key. 

5. The alias "taurus" sets up the environment for compilation and print services. 

6. Next, in the large terminal window type "cd srk" at the prompt and press the return 
key; this takes you to the Shepherd Real-time Kernel (-shepherd/srk) directory. While in 
srk you can modify or edit the require files with your favorite editor (e.g., xemacs or 
nedit). Once you have completed your work, save your files and compile. See figurel on 
the next page. 
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crndtiHil -/bin/c«h 

shepherd >cap 



tfAPNiNC ********** 

UNAUTHC«I2E0 USE Of THIS DEPAfiTfCNT OP DEFENSE (000) INTEREST 
COrVlJTER SYSTEM ANO/OR SOmtARE IS PRONI8ITED BY PUBLIC LAW. 

Tills Is a Oepartient of Defense coiputer systei. THIS COMPUTER 
SYSTEM. INCLUDING ALL RELATED EQUIPMENT. NETYfORKS AND NETWORK 
DEVICES (SPECIFICALLY INCLUDING INTERNET ACCESS). ARE PROVIDED 
ONLY FOR AUTHORIZED U.S. GOVERNfCKT USE. 000 Coiputer systees 
■ay be •onitored for all lawful purposes. Including to ensure 
that their use is authorized, for lanageient of the systei. to 
facilitate protection against unauthorized access, and to verify 
security procedures, survivability and operational security. 
Monitoring includes active attacks by authorized DOD entities 
to test or verify the security of this systea. During 
ionitorlng. inforiatlon aay be examined, recorded, copied and 
used for authorized purposes. All Inforaation. including 

C ersonal inforaation. placed on or sent over this systea aay 
e aonitored. USE OF THIS DOD C0»f>UTER SYSTEM. AUTHORIZED OR 
UNAUTHORIZED. CONSTITUTES CONSENT TO MONHORING OF THIS SYSTEM. 
Evidence of unauthorized use collected during aonltorlng aay 
be used for adai ni strati ve. criainal or adverse action. USE OF 
THIS SYSTEM CONSTITUTES CONSENT TO MOHITOfllNG FOR THESE PURPOSES. 

•••••• CLASSIFIED PROCESSING OH THIS SYSTEM IS PROHIBITED •••••• 

Last login: Sat 7u1 19 12:37:58 froa europa.cs.nps.na 



Welcoae to the Coaputer Science Departaent 
Sat Jul 19 DD:30;D1 PDT 1997 

You have aa1 1 . 
shepherd>taurus 
shepherd>cd srk 
/users/ res/shepherd/srk 
shepherd^ 




Fl>» EdK ARps Opnmt Durfon Toots C Motto Conwnfttxls 




Sinolud« *sl«>pher<i,b* 
#uwloda 'wivesent. b* 
#unclude *»ACh b* 
A^cluda *9«fvo h* 



/* * 

• Wwn ■* 

extern dox^U Dtive.Steer^Valoae 1. 

Dtivt_spe«d_iralue» 1. 

PraviooeCount^eed 1. 

PrmooeOeuntSpeed {. 

PrevioxitCountSceet 



mlar «bp(4Ii 

ctoubl* patbLength. thetabot. oexegaDot. epeedDot. 
void driver 0 

readCneodertO; /• Read Orive/Steot Kotors •/ 
eoa^AiteAotueiiucve <) i 

/•«Cciaiol»l«r>nveSp«dO. only for vheel epeed displaying */ 

acciauilet«Oriv*$t«er() . 

vq^taVOi 

celcuUUO, 

/* tsetOrivelO. •/ 
convex cLikit(); 
adwnceCountOi 



* Inltielise Koveeent- • 

*■ inualize Configuration and vehicle notion * 

void InlCXovenentO 

( 

xnc IX, 

■uxie • li 
OMga.Speed*0 0; 
teetcounter*0^ 
edCounter»0; 
patid,engtbaO.O> 

PrevioooCount^edlO I -9999WW j 
PrevioooOouat^ed j IJ -99999999 , 

PreviouaCountSpeedi 2} -99999999 ; 

Pr«viou«CountSpeed( 3} -99999999 ; 

PreviouaCountSteei (D1-99999999, 

PrsviouaCwmtSteei 111-99999999. 

—...gBace; mow.o (C 
fontifying' »ov» 0. . dona 







/bln/<3h /blo/cjh 



Figure 1: The Unix Workstation Environment 

7. Compilation is done through the use of a makefile. Hence, to compile all you must do 
is type "make comp" at the prompt and press the return key (this will either succeed or 
fail). If the compilation fails work the errors provided by the compiler and compile again 
(an iterative process). Once, the compilation is a success you are ready to FTP the S- 
records to the laptop. 



FTP S-Records to Laptop 

8. To begin to FTP the S-records to the laptop a few items must be accomplished. First, 
the robot power must be switched on (levers a, b, and c on the power supply in the "up" 
or closed position on the physical robot; provides power to the robot and charges the 
batteries). 
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See figure 2 (note the "up" position below represents the down position on the physical 
robot). 



Shepherd Rotary Vehicle 
Power Supply 




Figure 2; Power Supply Diagram 



Shepherd 
Rotaxy Robot 



9. Secondly, the laptop must be on and connected to the local network via the PCMIA 
card (ethemet). Press the laptop "On" button. 

10. Ensure the laptop powersupply is plugged in, and connected to the laptop. 

1 1 . Ensure the therenet cable is properly connected to the to the PC card. 

12. After booting our laptop will prompt you to login as guest-- just "click on the 
cancel” button. You should see the Windows desk top on the laptop (figure 3 below). 
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My Computer My Briefcase 



Shortcut to 



Neighborhood 





m 






Siwtcut (3) to 




InboK 


Ping 










Recycle Bin 


Q. t' 












' y ' ■ ' 




The Ir^ternet 






s 






The Microsoft 


Shortcut to 




Network 


ses2download 




Start 




1:20 .AM 




Figure 3: Windows Desk Top 



13. The screen print below describes the way the windows should look. Now double 
click on the WS_FTP95 shortcut to open the ftp tool. 
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<1^ 

BSSSH 




Nty Computef 


My Briefcase 

mm 




Network 

Neighborhood 

Jl* 


di 

Shortcut to 
Tehet 

B 




Inbox 


Shortcut (3) to 
Ping 










Recycle Bin 








j Vr P3b< 

I If ! 




The Internet 






jp 

The Microsoft 
Network 


QsS- 
Shortcut to 
$e$2downioad 


: > . : •- , • 1 


Start 




120 AM 



Figure 4: Windows WS_FTP95 Shortcut 



14. At this point the ftp tool opens. Click on the "OK" button. All information has been 
previously set for you(e.g., hostname, UserlD, and Password). See figure 5. 

15. The next Window has a split panel showing the Remote System (workstation) and 
the Local System (the laptop). The directories have been saved, so they always open to 
the correct directories. The file to be ftp'd is shepherd.TXT, it will be ftp'd from the 
remote system (-shepherd/srk) to the local system (c:\shepherdump). The file 
shepherd.TXT contains the S-Records that will be eventually downloaded to the actual 
robot CPU. To accomplish the ftp highlight the file to be transferred with your mouse 
and click on the arrow that points left (See figure 6). The file transfer usually takes 
about .3 seconds. 
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^ Start 



fT?WS_FTP95 LE 



1:21AM 



Figure 5: Windows WS_FTP95 Tool 
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dWS.FTPSS LE 131.120.1.13 



BOB 




'~R emote Sy$tem-^ 



QigDr 



Name 


Date 


|?S1 serial . h 


97C-I. 


serial . o 


97C 


^ servo . c 


97C 


servo . cT 


97C 


servo . h 


97C 


servo . o 


97C 


Kl shepherd 


97C 1 


shepherd . c 


97C 


[p shepherd . h 


97C— 1 


shepherd . h"^ 


97C 


shepherd . map 


97C 


shepherd . o 


97C 



MkDif 



View 



Exec 



Rename 



Delete 



Refresh 




r Auto 



Received 4478 bytes in 0.4 «cs, (109-1'^ f^bps). transfer succeeded 
226 T ransfer complete. 



Dose 



Cancel 



LogWnd 



He^ 



flptions 



^bout 



Eisjit 



^st«t 



ff?WS_FTP95 LE 131.1... 



1:23AM 



Figure 6: WS_FTP95 Tool File Transfer 



16. The file is now on the "hard" disk of the laptop. You can now close the window or 
kill the process by clicking on the appropriate button (active window:upper right comer 
area "X"). 
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Use Windows 95 HyperTerminal Program for Direct Connection 

17. You are now back at the Windows desk top. Now double click on the ses2download 
shortcut to open a hard-line, under Windows 95 HyperTerminal (See figure 7). 



m 



My Computer MyBrielcase 

1 



Network 

Ne^tnborkiood 



Shortcut to 
T elnet 



Inbox 

Recycle Bin 



A 

The Internet 



Shortcut (3) to 
Pii^S 



iw§”rtP3'g 



TteMicfOJoft Shortcut to 
Netvvofk sesidownbad 



ilQ Start 






Figure 7: Windows ses2download Shortcut 
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18. The next window to appear will be the open HyperTerminal window (See Figure 8). 
Press the "reset" button on the OMNIBYTE, Taurus board. The Taurus bug (debugger) 
prompt will appear in the HyperTerminal window (See Figure 8). 



se$2dov>inioad > HyperT erminal 



File £dil View Transfer Help 



□^1 



dIg»I mini ofl 



~I\\ 



Taurus Debugger/Diagnost^lcs Version 3.2 - 11/28/95 

FPC passed test 

Taurus_Bug>b£ 0 40000 0 

Effective address: 00000000 

Effective address: 0003FFFC 

Taurus_^Bug> 

lo 






“icrorin frAOC fX 






aQ start I 



IwrnSn 

TtpWS.FTPKLE 131.120.1. 









ses2download - Hype... 



1,:^% 1:25AM 



Figure 8: Windows HypertTerminal Window 



19. Ensure the lever on the switch box is placed on console (this allows the console to 
emulate a VT220). 

20. At the Taurus bug prompt type "bf 0 40000 0" and press the enter key. This 
command is called block fill by the debugger it allows you to disable the parity error 
interrupt {PEI) and prevents problems caused by uninitialized variables. See Figure 8. 



21. At the Taurus bug prompt type "lo" and press the enter key. The "lo" command 
initiates the download from the console. See Figure 8. 
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22. Next place the lever on the switch box is placed on host (this makes possible the use 
of the RS232 protocol to download shepherd.TXT from c:\shepherdump to the Taurus 
board). 

23. Click on the HyperTerminal "Transfer "option and choose the "Send Text File". 

All the "Send Text File" parameters have been previously set, so there is no action to 
take in that regard. 

See Figure 9. 







Effective address: 00000000 
Effective address: 0003FFFC 
Taurus_Bug> 



CaptueText. 

Taurus Debugger! 



FPC passed test 
Taurus_Bug>bf 0^ 



lo 




Capture lo £rntef 



m 3.2 - 11/28/95 




igastarti FTP W$_FTP95 LE 1 31 .1 20,1 ....| [ >e»2d(mnioad^^ - 



13 % 1;27AM 



Figure 9: Windows HypertTerminal Window 
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24. Now move to the root directory and select the c:\shepherdump directory, and double 
click on shepherd.TXT file. See figure 10. 



Q^;,se«2down!oaid • HypeiTenmnal 



file £dt View £dl Irensfei Help 



Send Text File 



Lookjn; I ^ shepheidump 



Fie name: jshephetd 



Fiesoljype; | Text file (“.TXT ) 



L?jx 

■3 Ml 



fipen I 



■3 



Cancel 



□MU 



3 



4 



I Trvfi l\/T<no 1 noon n k.t 1 Icront 1 Itadc luVai fr i 



Start I npWS_FTP95LE 131.1 20.1 ....| jjl^tee2download - Hype... 

Figure 10: Send File From shepherdump 



1:29 AM 



The download process is in motion. The "red" transmit light on the RS232 connector to 
the switch box will become faint while transmission is in progress. Once the 
transmission is complete the "red" transmit light on the RS232 connector to the switch 
box will become a constant red; the Hyperterminal window will pause during the 
transmission process. The Taurus bug prompt will appear in the HyperTerminal 
window after the transmission is complete. 

25. Ensure the lever on the switch box is placed on console (this allows the console to 
emulate a VT220). 
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25. Now type "go ” at the Taurus bug prompt and press the enter key (see figure 1 1 ). 
The program that you have previously downloaded will be executed. 



ses2download - Hyper! erminal E3 



Fie £d^ V«w £*8 Iransfer Help 





jg9Start| rtPWS_RP95 11131.120.1... J|^ses2download - Hype 



1:31AM 



Figure 11: Taurus Bug Prompt Returns After Transmisssion Completion 
and the "go" Command is given to Execute the Program. 
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After the "go” has been given and the execution begins the Shepherd Main Menu 
appears for your selection. 



^ ses2download < HvperT eiminal 



file £dt VjJiew £all Iransfer Help 



d|g»I ^\B\ ii*l 






SHEPHERD Hain Henu (Las^ Update: 
1 5 0 5 0 5 0— 



27 Feb 97) 

__5 0 5- 



Please choose: 



Diagnostics 



(1) Stop 

(2) Straight Hot ion (Autonomous) 

(3) Straight Hotion by Joystick 

(4) XY-Hotion by Joystick 

(5) Rotate 

(6) Sinusoidal Hotion 

(7) Tornado (External Center of Rotation) 

(8) Tornado (Internal Center of Rotation) 

(9) Align Wheels 
(0) Exit Program 



'4 



“irADC iNiiiu frCZrm lDr.« 



. 1 . j 






ii 9 Staft| FTP L£ 1 31 . 1 20.1 .«..| [l|^tes 2 download - Hype... 



1;33AM 



Figure 12; Shepherd Main Menu 



Note; remember this is a quick guide and does not provided answers to questions 
concerning file size, debugger commands, and other requirements or constraints. 
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APPENDIX L: SENSING SIMULATION DATA 



Lstance 


X 


y 


theta 


psi 


0.000 


0.400 


0.000 


0.001 


2.355 


0.400 


0.800 


0.001 


0.002 


2.353 


0.800 


1.200 


0.002 


0.005 


2.352 


1.200 


1.600 


0.005 


0.008 


2.350 


1.600 


2.000 


0.008 


0.011 


2.348 


2.000 


2-400 


0.014 


0.015 


2.347 


2.400 


2.800 


0.021 


0.020 


2.345 


2.800 


3.200 


0.029 


0.025 


2.344 


3.200 


3.600 


0.041 


0.031 


2.342 


3.600 


3.999 


0.054 


0.037 


2.341 


4.000 


4.399 


0.070 


0.043 


2.339 


4.400 


4.799 


0.088 


0.050 


2.337 


4.800 


5.198 


0.110 


0.057 


2.336 


5.200 


5.597 


0.134 


0.064 


2.334 


5.600 


5.996 


0.161 


0.072 


2.333 


6.000 


6.395 


0.192 


0.080 


2.331 


6.400 


6.794 


0.225 


0.088 


2.330 


6.800 


7.192 


0.262 


0.096 


2.328 


7.200 


7.590 


0.302 


0.105 


2.326 


7.600 


7.988 


0.346 


0.114 


2.325 


8.000 


8.385 


0.393 


0.123 


2.323 


8.400 


8.782 


0.444 


0.131 


2.322 


8.800 


9.178 


0.498 


0.141 


2.320 


9.200 


9.574 


0.556 


0.150 


2.319 


9.600 


9.969 


0.617 


0.159 


2.317 
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10.000 


10.364 


0.682 


0.168 


2.315 


10.400 


10.758 


0.751 


0.177 


2.314 


10.800 


11.151 


0.823 


0.187 


2.312 


11.200 


11.544 


0.899 


0.196 


2.311 


11.600 


11.936 


0.979 


0.205 


2.309 


12.000 


12.327 


1.062 


0.214 


2.308 


12.400 


12.718 


1.149 


0.224 


2.306 


12.800 


13.107 


1.240 


0.233 


2.304 


13.200 


13.496 


1.334 


0.242 


2.303 


13.600 


13.884 


1.431 


0.251 


2.301 


14.000 


14.271 


1.532 


0.260 


2.300 


14.400 


14.657 


1.637 


0.269 


2.298 


14.800 


15.042 


1.745 


0.278 


2.297 


15.200 


15.426 


1.856 


0.286 


2.295 


15.600 


15.810 


1.971 


0.295 


2.293 


16.000 


16.192 


2.089 


0.303 


2.292 


16.400 


16.573 


2.210 


0.312 


2.290 


16.800 


16.953 


2.334 


0.320 


2.289 


17.200 


17.332 


2.461 


0.328 


2.287 


17.600 


17.711 


2.592 


0.336 


2.286 


18.000 


18.088 


2.725 


0.344 


2.284 


18.400 


18.464 


2.862 


0.352 


2.282 


18.800 


18,839 


3.001 


0.359 


2.281 


19.200 


19.213 


3.143 


0.367 


2.279 


19.600 


19.585 


3.288 


0.374 


2.278 


20.000 


19.957 


3.435 


0.381 


2.276 


20.400 


20.328 


3.586 


0.388 


2.275 


20.800 


20.698 


3.738 


0.395 


2.273 


21.200 


21.066 


3.893 


0.402 


2.271 
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21.600 


21.434 


4.051 


0.408 


2.270 


22.000 


21.801 


4.211 


0.415 


2.268 


22.400 


22.166 


4.373 


0.421 


2.267 


22.800 


22.531 


4.538 


0.427 


2.265 


23.200 


22.894 


4.704 


0.433 


2.264 


23.600 


23.257 


4.873 


0.438 


2.262 


24.000 


23.619 


5.044 


0.444 


2.260 


24.400 


23.980 


5.217 


0.449 


2.259 


24.800 


24.339 


5.391 


0.454 


2.257 


25.200 


24.698 


5.568 


0.459 


2.256 


25.600 


25.056 


5.746 


0.464 


2.254 


26.000 


25.414 


5.926 


0.469 


2.253 


26.400 


25.770 


6.108 


0.474 


2.251 


26.800 


26.126 


6.291 


0.478 


2.249 


27.200 


26.480 


6.476 


0.482 


2.248 


27.600 


26.834 


6.662 


0.486 


2.246 


28.000 


27.188 


6.849 


0.490 


2.245 


28.400 


27.540 


7.038 


0.494 


2.243 


28.800 


27.892 


7.229 


0.497 


2.242 


29.200 


28.243 


7.420 


0.501 


2.240 


29.600 


28.594 


7.613 


0.504 


2.238 


30.000 


28.944 


7.807 


0.507 


2.237 


30.400 


29.293 


8.001 


0.510 


2.235 


30.800 


29.642 


8.197 


0.513 


2.234 


31.200 


29.990 


8.394 


0.516 


2.232 


31.600 


30.338 


8.592 


0.518 


2.231 


32.000 


30.685 


8.790 


0.521 


2.229 


32.400 


31.032 


8.990 


0.523 


2.227 


32.800 


31.378 


9.190 


0.525 


2.226 
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33.200 


31.724 


9.390 


0.527 


2.224 


33.600 


32.070 


9.592 


0.529 


2.223 


34.000 


32.415 


9.794 


0.530 


2.221 


34.400 


32.760 


9.996 


0.532 


2.220 


34.800 


33.105 


10.200 


0.533 


2.218 


35.200 


33.449 


10.403 


0.535 


2.216 


35.600 


33.793 


10.607 


0.536 


2.215 


36.000 


34.137 


10. 8U 


0.537 


2.213 


36.400 


34.481 


11.016 


0.538 


2.212 


36.800 


34.824 


11.221 


0.538 


2.210 


37.200 


35.167 


11.426 


0.539 


2.209 


37.600 


35.511 


11.632 


0.540 


2.207 


38.000 


35.854 


11.837 


0.540 


2.205 


38.400 


36.197 


12.043 


0.540 


2.204 


38.800 


36.540 


12.249 


0.541 


2.202 


39.200 


36.883 


12.455 


0.541 


2.201 


39.600 


37.225 


12.661 


0.541 


2.199 


40.000 


37.568 


12.867 


0.541 


2.198 


40.400 


37.911 


13.073 


0.541 


2.196 


40.800 


38.254 


13.279 


0.540 


2.194 


41.200 


38.597 


13.484 


0.540 


2.193 


41.600 


38.941 


13.690 


0.539 


2.191 


42.000 


39.284 


13.895 


0.539 


2.190 


42.400 


39.627 


14.100 


0.538 


2.188 


42.800 


39.971 


14.305 


0.537 


2.187 


43.200 


40.315 


14.510 


0.537 


2.185 


43.600 


40.658 


14.714 


0.536 


2.183 


44.000 


41.002 


14.918 


0.535 


2.182 


44.400 


41.347 


15.122 


0.534 


2.180 
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44.800 


41.691 


15.325 


0.532 


2.179 


45.200 


42.036 


15.528 


0.531 


2.177 


45.600 


42.381 


15.730 


0.530 


2.176 


46.000 


42.726 


15.932 


0.528 


2.174 


46.400 


43.072 


16.133 


0.527 


2.172 


46.800 


43.418 


16.334 


0.525 


2.171 


47.200 


43.764 


16.535 


0.524 


2.169 


47.600 


44.111 


16.734 


0.522 


2.168 


48.000 


44.457 


16.934 


0.521 


2.166 


48.400 


44.805 


17.132 


0.519 


2.165 


48.800 


45.152 


17.330 


0.517 


2.163 


49.200 


45.500 


17.528 


0.515 


2.161 


49.600 


45.848 


17.724 


0.513 


2.160 


50.000 


46.197 


17.920 


0.511 


2.158 


50.400 


46.546 


18.116 


0.509 


2.157 


50.800 


46.896 


18.310 


0.507 


2.155 


51.200 


47.246 


18.504 


0.505 


2.154 


51.600 


47.596 


18.697 


0.503 


2.152 


52.000 


47.947 


18.889 


0.500 


2.150 


52.400 


48.298 


19.081 


0.498 


2.149 


52.800 


48.649 


19.272 


0.496 


2.147 


53.200 


49.002 


19.461 


0.493 


2.146 


53.600 


49.354 


19.650 


0.491 


2.144 


54.000 


49.707 


19.839 


0.489 


2.143 


54.400 


50.060 


20.026 


0.486 


2.141 


54.800 


50.414 


20.212 


0.484 


2.139 


55.200 


50.769 


20.398 


0.481 


2.138 


55.600 


51.124 


20.582 


0.478 


2.136 


56.000 


51.479 


20.766 


0.476 


2.135 
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56.400 


51.835 


20.949 


0.473 


2.133 


56.800 


52.191 


21.131 


0.470 


2.132 


57.200 


52.548 


21.311 


0.468 


2.130 


57.600 


52.905 


21.491 


0.465 


2.128 


58.000 


53,263 


21.670 


0.462 


2.127 


58.400 


53.621 


21.848 


0.460 


2.125 


58.800 


53.980 


22,025 


0.457 


2.124 


59.200 


54.339 


22.201 


0.454 


2.122 


59.600 


54.699 


22.376 


0.451 


2.121 


60.000 


55.059 


22.550 


0.448 


2.119 


60,400 


55.420 


22.723 


0.445 


2.117 


60.800 


55.781 


22.894 


0.442 


2.116 


61,200 


56.143 


23.065 


0.440 


2,114 


61.600 


56.505 


23.235 


0.437 


2.113 


62.000 


56.868 


23.403 


0.434 


2.111 


62.400 


57.231 


23.571 


0.431 


2.110 


62.800 


57,595 


23.738 


0.428 


2.108 


63.200 


57.959 


23.903 


0.425 


2.106 


63.600 


58.323 


24.067 


0.422 


2.105 


64.000 


58.689 


24.231 


0,419 


2.103 


64.400 


59.054 


24.393 


0.416 


2.102 


64.800 


59.420 


24.554 


0.413 


2.100 


65.200 


59.787 


24.714 


0.410 


2.099 


65.600 


60.154 


24.873 


0.407 


2.097 


66.000 


60.522 


25.030 


0.404 


2.095 


66.400 


60.890 


25.187 


0.401 


2.094 


66.800 


61.258 


25.343 


0.398 


2.092 


67,200 


61.627 


25.497 


0.395 


2.091 


67.600 


61.997 


25.650 


0.392 


2.089 
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68.000 


62.367 


25.802 


0.389 


2.088 


68.400 


62.737 


25.953 


0.386 


2.086 


68.800 


63.108 


26.103 


0.383 


2.084 


69.200 


63.479 


26.252 


0.380 


2.083 


69.600 


63.851 


26.400 


0.377 


2.081 


70.000 


64.223 


26.546 


0.374 


2.080 


70.400 


64.596 


26.692 


0.371 


2.078 


70.800 


64.969 


26.836 


0.368 


2.077 


71.200 


65.342 


26.979 


0.365 


2.075 


71.600 


65.716 


27.121 


0.362 


2.073 


72.000 


66.091 


27.262 


0.358 


2.072 


72.400 


66.465 


27.402 


0.355 


2.070 


72.800 


66.841 


27.541 


0.352 


2.069 


73.200 


67.216 


27.678 


0.349 


2.067 


73.600 


67.592 


27.815 


0.347 


2.066 


74.000 


67.969 


27.950 


0.344 


2.064 


74.400 


68.346 


28.084 


0.341 


2.062 


74.800 


68.723 


28.217 


0.338 


2.061 


75.200 


69.100 


28.349 


0.335 


2.059 


75.600 


69.478 


28.480 


0.332 


2.058 


76.000 


69.857 


28.610 


0.329 


2.056 


76.400 


70.236 


28.738 


0.326 


2.055 


76.800 


70.615 


28.866 


0.323 


2.053 


77.200 


70.994 


28.992 


0.320 


2.051 


77.600 


71.374 


29.117 


0.317 


2.050 


78.000 


71.754 


29.241 


0.314 


2.048 


78.400 


72.135 


29.365 


0.311 


2.047 


78.800 


72.516 


29.487 


0.308 


2.045 


79.200 


72.897 


29.607 


0.306 


2.044 



259 



79.600 


73.279 


29.727 


0.303 


2.042 


80.000 


73.661 


29.846 


0.300 


2.040 


80.400 


74.043 


29.964 


0.297 


2.039 


80.800 


74.426 


30.080 


0.294 


2.037 


81.200 


74.809 


30.196 


0.292 


2.036 


81.600 


75.192 


30.310 


0.289 


2.034 


82.000 


75.576 


30.424 


0.286 


2.033 


82.400 


75.959 


30.536 


0.283 


2.031 


82.800 


76.344 


30.647 


0.281 


2.029 


83.200 


76.728 


30.757 


0.278 


2.028 


83.600 


77.113 


30.867 


0.275 


2.026 


84.000 


77.498 


30.975 


0.272 


2.025 


84.400 


77.883 


31.082 


0.270 


2.023 


84.800 


78.269 


31.188 


0.267 


2.022 


85.200 


78.655 


31.293 


0.264 


2.020 


85.600 


79.041 


31.397 


0.262 


2.018 


86.000 


79.428 


31.500 


0.259 


2.017 


86.400 


79.815 


31.602 


0.257 


2.015 


86.800 


80.202 


31.703 


0.254 


2.014 


87.200 


80.589 


31.803 


0.251 


2.012 


87.600 


80.976 


31.902 


0.249 


2.011 


88.000 


81.364 


32.000 


0.246 


2.009 


88.400 


81.752 


32.097 


0.244 


2.008 


88.800 


82.141 


32.193 


0.241 


2.006 


89.200 


82.529 


32.289 


0.239 


2.004 


89.600 


82.918 


32.383 


0.236 


2.003 


90.000 


83.307 


32.476 


0.234 


2.001 


90.400 


83.696 


32.568 


0.232 


2.000 


90.800 


84.085 


32.659 


0.229 


1.998 
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91.200 


84.475 


32.750 


0.227 


1.997 


91.600 


84.865 


32.839 


0.224 


1.995 


92.000 


85.255 


32.928 


0.222 


1.993 


92.400 


85.645 


33.015 


0.220 


1.992 


92.800 


86.036 


33.102 


0.217 


1.990 


93.200 


86.427 


33.188 


0.215 


1.989 


93.600 


86.817 


33.273 


0.213 


1.987 


94.000 


87.209 


33.357 


0.210 


1.986 


94.400 


87.600 


33.440 


0.208 


1.984 


94.800 


87.991 


33.522 


0.206 


1.982 


95.200 


88.383 


33.603 


0.204 


1.981 


95.600 


88.775 


33.684 


0.201 


1.979 


96.000 


89.167 


33.763 


0.199 


1.978 


96.400 


89.559 


33.842 


0.197 


1.976 


96.800 


89.951 


33.920 


0.195 


1.975 


97.200 


90.344 


33.997 


0.193 


1.973 


97.600 


90.736 


34.073 


0.191 


1.971 


98.000 


91.129 


34.149 


0.189 


1.970 


98.400 


91.522 


34.223 


0.186 


1.968 


98.800 


91.915 


34.297 


0.184 


1.967 


99.200 


92.309 


34.370 


0.182 


1.965 


99.600 


92.702 


34.442 


0.180 


1.964 


100.000 


93.096 


34.513 


0.178 


1.962 


100.400 


93.489 


34.584 


0.176 


1.960 


100.800 


93.883 


34.654 


0.174 


1.959 


101.200 


94.277 


34.723 


0.172 


1.957 


101.600 


94.671 


34.791 


0.170 


1.956 


102.000 


95.066 


34.858 


0.168 


1.954 


102.400 


95.460 


34.925 


0.166 


1.953 
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102.800 


95.855 


34.991 


0.165 


1.951 


103.200 


96.249 


35.056 


0.163 


1.949 


103.600 


96.644 


35.120 


0.161 


1.948 


104.000 


97.039 


35.184 


0.159 


1.946 


104.400 


97.434 


35.247 


0.157 


1.945 


104.800 


97.829 


35.309 


0.155 


1.943 


105.200 


98.224 


35.371 


0.154 


1.942 


105.600 


98.620 


35.432 


0.152 


1.940 


106.000 


99.015 


35.492 


0.150 


1.938 


106.400 


99.411 


35.551 


0.148 


1.937 


106.800 


99.806 


35.610 


0.146 


1.935 


107.200 


100.202 


35.668 


0.145 


1.934 


107.600 


100.598 


35.725 


0.143 


1.932 


108.000 


100.994 


35.782 


0.141 


1.931 


108.400 


101.390 


35.838 


0.140 


1.929 


108.800 


101.786 


35.893 


0.D8 


1.927 


109.200 


102.182 


35.948 


0.D6 


1.926 


109.600 


102.579 


36.002 


0.D5 


1,924 


110.000 


102.975 


36.056 


0.D3 


1.923 


110.400 


103.372 


36.108 


0.D2 


1.921 


110.800 


103.768 


36.161 


O.DO 


1.920 


111.200 


104.165 


36.212 


0.D8 


1.918 


111.600 


104.562 


36.263 


0.D7 


1.916 


112.000 


104.959 


36. 3D 


0.D5 


1.915 


112.400 


105.355 


36.363 


0.D4 


1.9D 


112.800 


105.752 


36, 4D 


0.122 


1.9D 


113.200 


106.149 


36.461 


o.m 


1.910 


113.600 


106.547 


36.509 


0.119 


1.909 


114.000 


106.944 


36.556 


0.118 


1.907 
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114,400 


107.341 


36.603 


0.116 


1.905 


114.800 


107.738 


36.649 


0.115 


1.904 


115.200 


108.136 


36.695 


0.114 


1.902 


115.600 


108.533 


36.740 


0.112 


1.901 


116.000 


108.931 


36.784 


0.111 


1.899 


116.400 


109.328 


36.828 


0.109 


1.898 


116.800 


109.726 


36.872 


0.108 


1.896 


117.200 


110.124 


36.915 


0.107 


1.894 


117.600 


110.521 


36.957 


0.105 


1.893 


118.000 


110.919 


36.999 


0.104 


1.891 


118.400 


111.317 


37.040 


0.103 


1.890 


118.800 


111.715 


37.081 


0.102 


1.888 


119.200 


112.113 


37.121 


0.100 


1.887 


119.600 


112.511 


37.161 


0.099 


1.885 


120.000 


112.909 


37.200 


0.098 


1.883 


120.400 


113.307 


37.239 


0.097 


1.882 


120.800 


113.705 


37.277 


0.095 


1.880 


121.200 


114.103 


37.315 


0.094 


1.879 


121.600 


114.502 


37.353 


0.093 


1.877 


122.000 


114.900 


37.389 


0.092 


1.876 


122.400 


115.298 


37.426 


0.091 


1.874 


122.800 


115.697 


37.462 


0.089 


1.872 


123.200 


116.095 


37.497 


0.088 


1.871 


123.600 


116.494 


37.532 


0.087 


1.869 


124.000 


116.892 


37.567 


0.086 


1.868 


124.400 


117.291 


37.601 


0.085 


1.866 


124.800 


117.689 


37.635 


0.084 


1.865 


125.200 


118.088 


37.668 


0.083 


1.863 


125.600 


118.486 


37.701 


0.082 


1.861 
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APPENDIX M: SENSING SIMULATION CODE (MAIN2.CC) 



// File: main2.cc 
// Name: Edward Mays 
// Sensing Simulation 
// Unix 
//GCC 

// Date: 26 August 1997 
// 

// Description 

// THIS PROGM SIMULATES THE MOVEMENT OF A SQARE OBJECT ALONG A 
//PATH. THE OBJECT’S PATH DIRECTION (THETA) IS CHANGING, AS IS THE 
//OBJECTS ORIENTATION (PSI). LINE TRACKING IS USED AND THE X-AXIS IS 
THE //REFERENCE LINE. THE REFERENCE LINE IS INCREMENTED BY 40 
UNITS IN THE 

// 

// Header file info 

// 

#include <iostream.h> 

#include <math.h> 

#include <fstream.h> 

#include <stdio.h> 



#definePI 3.14159265358979323846 
#define RAD 57.2957795 1 308232087684 
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double deltaTime = 0.01 ;// 0.01 

double Vel = 40.0; 

double omega = -0. 1570796327; 

FILE *f0, *f 1 , *f2, *f3, *f4, *f5,*f6 ; //PTR TO FILE FOR OUTPUT DATA 



//structure to hold configuration including x, y, theta, and kappa 

typedef struct { 
double x; 
double y; } 

POINT; 

typedef struct { 

POINT Point; 
double Theta; 
double Kappa; 
double Psi; 

) 

CONFIGURATION; 

// 

//Function: GetSmooth 
//Return Value:n/a 
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//Parameters: function parm list 
//Purpose: gets users input for sO/smoothness 

// 

double GetSmooth(double &s0) 

{ 



cout « "enter your value for smoothness (negatives not allowed)" «endl; 
cin » sO; 
return (sO >= 0.0); 

)// GetSmooth 

// 

//Function: InitConfig 
//Return Value:n/a 
//Parameters: function parm list 
//Purpose: SETS INITIAL CONFIGURATION 
// 

void InitConfig(CONFIGURATION& q_init, CONFIGURATION& 
q_xaxis,CONFIGURATION& qbody,CONFIGURATION& qfrontR, 

CONFIGURATION& qfrontL, CONFIGURATION& qrearR, CONFIGURATION& 
qrearL, CONFIGURATION& qsnapshot, 

double &s0, double &deltaS) 

{ 

cout«"Setting the initial configuration" 
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«"x=0, y = 0, theta = 0, and kappa = 0 "«endl; 



q_init.Point.x = 0.0; 
q_init.Point.y = 0.0; 
q_init. Theta = 0.0; 
q_init.Kappa = 0.0; 

qJnit.Psi = 2.3562 1 9449; I* 3*PI/4.0 *! 

cout«"Setting the reference line configuration" 
«"x = 0, y = 40, theta = 0 , and kappa = 0 "«endl; 

q_xaxis.Point.x = 0.0; 
q_xaxis.Point.y =40.0; 
q_xaxis.Theta = 0.0; 
q_xaxis.Kappa = 0.0; 



//individual wheels 
qfrontR.Point.x = 40; /* wheel 1 *! 
qfrontR.Point.y = -40; 

qfrontL.Point.x = 40; /* wheel2 */ 
qfrontL.Point.y = 40; 

qrearR.Point.x = -40; /* wheel3 */ 
qrearR.Point.y = -40; 
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qrearL.Point.x = -40; /* wheel 4 */ 
qrearL.Point.y = 40; 



qsnapshot.Pointx = 0.0; 
qsnapshot.Pointy = 0.0; 

cout«"Enter size constant for smoothness <retum>"«endl; 
GetSmooth(sO); 

cout«"Entering Step size constant deltaS(deltaS=Vel*deltaT)."«endl; 
deltas = Vel*deltaTime;//.05 orig 



}// InitConfig 



// 

//Function; CreateConst 
//Return Valuern/a 
//Parameters: function parm list 
//Purpose: create constants for 
// steering function dk/ds 
// 

void CreateConst(double &a, double &b, double &c, double &s0) 
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{ 

double k; 

k = 1 .0/sO; //all consts by def, including curvature 
a = 3.0*k; 
b = 3.0*k*k; 
c = k*k*k; 

}// CreateConst 



// — 

//Function; GetSteerL 
//Return Value;n/a 
//Parameters: function parm list 

//Purpose: lambda=dk/ds (LINEAR STEERING FUNCTION) 

// 

double GetSteerL(double &a, double &b, double &c, CONFIGURATION& q, 
CONFIGURATION* q_xaxis) 

{ 
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double delta_r; 



delta_r = -(q.Point.x - q_xaxis.Point.x)*sin(q_xaxis.Theta) + 
(q.Point.y - q_xaxis.Point.y)*cos(q_xaxis.Theta); 

return (-(a*q. Kappa + b*(q.Theta - q_x ax is. Theta) + c*delta_r)); 
} //GetSteerL 



// 

//Function: GetDeltakappa 
//Return Value:n/a 
//Parameters: function parm list 

//Purpose: DETERMINES THE KAPPA DIFFERENCE PER INCREMENT OF S 
// 

double GetDeltakappa(double &Dk_Ds, double &deltaS, double &deltakappa) 

{ 

deltakappa = Dk_Ds*deltaS; 
retum(deltakappa); 

} //GetDel takappa 
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// 



//Function: retumkappa 
//Return Value:n/a 
//Parameters: function parm list 

//Purpose: CALCULATES NEW VALUE FOR KAPPA USING deltaK 
// 



CONFIGURATION retumkappa(double &deltakappa, CONFIGURATION &q) 

{ 

q. Kappa = q. Kappa + deltakappa; 
return q; 

} //retumkappa 



// 

//Function: GetS 
//Return Value:n/a 
//Parameters: function parm list 

//Purpose: INCREMENTS S THROUGH EACH ITERATION OF THE WHILE LOOP 
// 



double GetS(double &s, double &deltaS) 

{ 
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s = s + deltas ; 
return s; 

}//GetS 

// 

//Function; GetDeltaTheta 
//Return Value:n/a 
//Parameters: function parm list 

//Purpose: COMPUTES CHANGE IN THETA PER INCREMENT OF S 
// 

double GetDeltaTheta(CONFIGURATION &q, double &deltaS, double &deltaT) 

{ 

deltaT = q.Kappa*deltaS; 
retum(deltaT); 

} //GetDeltaTheta 

// 

//Function: Circ 
//Return Value:n/a 
//Parameters: function parm list 
//Purpose: Circ function from notes 6.29 
// 
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void Circ(double Length, double alpha, CONFIGURATION &q) 

{ 

double alpha2, alpha4; 
alpha2=alpha*alpha; 
alpha4=alpha2*alpha2; 



//configuration ql 

q.Point.x = (1.0 - alpha2/6.0 + alpha4/120.0)*Length; 
q.Point.y = (0.5 - alpha2/24.0 + alpha4/720.0)*Length*alpha; 
q. Theta = alpha; 



}//Circ 



// 

//Function: Compose 
//Return Value:n/a 
//Parameters: function parm list 

//Purpose: updates the configuration and computes new config (notes 6.2) 

// 

CONFIGURATION Compose(CONFIGURATION& ql,CONFIGURATION& 
q2,CONFIGURATION& q3, double& s,double& deltaTime) 
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{ double x,y, 
sinTheta = sin(ql. Theta), 
cosTheta = cos(q 1 .Theta); 

X = ql.Point.x + q2.Point.x*cosTheta - q2.Point.y*sinTheta; 
y = ql .Point. y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 
qS.Point.x = x; 
qS.Point.y = y; 

q3 .Theta = q 1 .Theta + q2.Theta; 

qS.Psi = ql.Psi + (omega * deltaTime); /* how to handle move left/right? */ 

fprintf(f6,"%10.3f%10.3f%10.3f %10.3f %10.3f\n", 
s,q3.Point.x, q3.Point.y,q3.Theta, q3.Psi); 
return q3; 

}// end Compose 



CONFIGURATION Compose2(CONFIGURATION& ql,CONFIGURATION& q2, 
CONFIGURATION& q3) /^position */ 



{ double x,y, 
sinTheta = sin(q 1 .Psi), 
cosTheta = cos(q 1 .Psi); 
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X = q 1 .Point.x + q2.Point.x*cosTheta - q2.Point.y*sinTheta; 
y = ql. Point. y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 

q3. Point.x = x; 
qS.Point.y = y; 
return q3; 

}//end Compose2 



// 

//Function: Openfile 
//Return Value :n/a 
//Parameters: function parm list 
//Purpose: To compute transposition 

// 

void OpenfileO 

{ 

fO = fopenrdrk.dat","w"); 
fl = fopen("wheell.dat","w"); 
f2 = fopen("wheel2.dat","w"); 
f3 = fopen("wheel3.dat","w"); 
f4 = fopen("wheel4.dat","w"); 
f5 = fopenCcomposite.dat", "w"); 
f6 = fopenCpsi.dat", "w"); 

}// Openfile 
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// 



//Function: Print 
//Return Value:n/a 
//Parameters: function parm list 
//Purpose: To compute transposition 

// 

void printFiIe(FILE *f, CONFIGURATION &q) 

{ 

fprintf(f,"%10.3f 

q.Point.x, q.Point.y); 



}// printFile 



// 

//Function: blankLine 
//Return Value: n/a 
//Parameters: function parm list 
//Purpose: To compute transposition 

// 

void blankLine(FILE *f) 

{ 
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fprintf(f,"\n"); 



}// blankLine 



// 

//Function; update Wheels 
//Return Value:n/a 
//Parameters: function parm list 
/Purpose: create constants for 
// steering function dk/ds 
// 

void updateWheels(CONFIGURATION& qbody, CONFIGURATION& qfrontR, 
CONFIGURATION& qfrontL, CONFIGURATION qrearR, CONFIGURATION qrearL, 
CONFIGURATION& qwheell, CONFIGURATION& qwheel2, CONFIGURATION& 
qwheeB, CONFIGURATION«& qwheel4, CONFIGURATION& q3,int& s2) 

{ printFile(fO,qbody); 

qwheel 1 = Compose2(qbody,qfrontR,q3); 

printFile(f 1 ,qwheel 1 ); 

blankLine(fl); 

qwheeB = Compose2(qbody,qfrontL,q3); 
printFile(f2,qwheel2); 
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blankLine(f2); 

qwheelS = Compose2(qbody, qrearR, q3); 

printFile(f3,qwheel3); 

blankLine(f3); 

qwheeW = Compose2(qbody, qrearL,q3); 

printFile(f4,qwheel4); 

blankLine(f4); 

if((s2==0.0)ll(s2% 1 00==0)) { 

printFile(f5,qwheel 1); 
printFi le(f5 ,qwheel2) ; 
printFile(f5,qwheel4); 
printFile(f5,qwheel3); 
printFile(f5,qwheell); 
blankLine(f5); 

) 

}// updateWheels 



int main() 
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{ CONFIGURATION q, q_xaxis, New_q, qbody, qfrontR, qfrontL, qrearR, qrearL, 
qwheell, qwheel2, qwheelS, qwheel4, qsnapshot,q3; 

int ix,s2, counter; 
double a, b, c; 
double s,sO; 
double Dk_Ds; 
double deltas ; 
double deltaT; 
double deltaK; 
const double Sdig = 0.001 ; 
double smax =400.0; 

InitConfig(q, q_xaxis, qbody, qfrontR, qfrontL, qrearR, qrearL,qsnapshot,sO, deltaS); 
//configuaration set up 

OpenfileO; 

printFile(f0,q); 

printFile(fl, qfrontR); //write initial config to file 

printFile(f2,qfrontL); 
printFile(f3,qrearR); 
printFile(f4,qrearL); 

//printFile(f5,qfrontR); 

//printFile(f5,qfrontL); 



//constants equation 6.3 

//sO is smoothness, s is the incremental step 



//const used for prec/toler 
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//printFile(f5,qrearL); 

//printFile(f5,qrearR); 

printFile(f5,qfrontR); 

blankLine(f5); 

CreateConst(a, b, c, sO); //calcs consts 

s = 0.0; 

s2=s; 

counter=0; 

for(ix=0; ix<10;ix++){ 
do 
{// 

Dk_Ds = GetSteerL(a, b, c, q, q_xaxis); //calculates lambda =dk/ds 
deltaK = GetDeltakappa(Dk_Ds, deltaS, deltaK); //lambda*deltaS 
retumkappa(deltaK, q); //Kappa <= kappa + deltaK 

deltaT = GetDeltaTheta(q, deltaS, deltaT); //Theta <= Theta + deltaT 
Circ(deltaS, deltaT, New_q); //cir 

qbody = Compose(q, New_q, q, s, deltaTime); //compose 

updateWheels(qbody,qfrontR, qfrontL, qrearR, 

qrearL,qwheel 1 , qwheel2, qwheeB, 
qwheel4,q3,s2); 

GetS(s, deltaS); 
s2=s; 
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} while (s<smax); 

// }while ((fabs(q.Point.y) > Sdig)ll(fabs(q.Theta) > Sdig) II 

// (fabs(q.Kappa) > Sdig)); 

s=0.0; 

s2=s; 

q_xaxis. Point. y = q_xaxis.Point.y + 40.0; 

if(ix%2==0){ 
q_xaxis.Theta = PI; 
q.Theta = PI; 
omega = fabs(omega); 

}else{ 

q_xaxis.Theta = 0.0; 
q.Theta = 0.0; 
omega = -omega; 

} 

} 

fclose(fO); 
fclose(fl); 
fclose(f2); 
fclose(O); 
fclose(f4); 
fclose(f5); 
fclose(f6); 
return 0; 

}//end main2.cc 
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APPENDIX N: INPUT VS. OUPUT VELOCITY 



-1024 - 87.657 
-1023 - 87.429 
-1020 - 86.975 
-1015 - 86.747 
- 1010 - 86.406 
-1005 - 85.838 
- 1000 - 85.383 
-900 - 76.856 
-800 - 67.998 
-700 - 59.082 
- 600 - 51.048 
-500 - 42.748 
- 400 - 34.107 
-375 - 31.834 
-350 - 29.673 
-300 - 25.580 
-250 - 21.260 
- 225 - 19.100 
- 200 - 17.053 
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- 175 - 14.780 



- 150 - 12.506 
- 125 - 10.459 
- 100 - 8.413 
-90 - 7.503 
-80 - 6.707 
-70 - 5.798 
-60 - 5.002 
-50 - 4.092 
-40 - 3.297 
-30 - 2.387 
- 20 - 1.591 
- 10 -.682 
-5 -.341 
-1 -.113 
0.0 0.0 
1023 87.429 
1020 86.975 
1015 86.747 
1010 86.406 
1005 85.838 
1000 85.383 
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900 76.856 



800 67.998 
700 59.082 
600 51.048 
500 42.748 
400 34.107 
375 31.834 
350 29.673 
300 25.580 
250 21.260 
225 19.100 
200 17.053 
175 14.780 
150 12.506 
125 10.459 
100 8.413 
90 7.503 
80 6.707 
70 5.798 
60 5.002 
50 4.092 
40 3.297 
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30 2.387 



20 1.591 
10 .682 
5.341 
1 .113 
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APPENDIX O: INPUT VS OUTPUT STEERING RATES 



A. DESIRED INPUT RATE VS ACTUAL (BOTH ESTIMATED AND 
SOFTWARE DEPENDENT) 



Desired rate of 
turn 


Time 

Stop watch 
(sec) 


Estimated Rate 
(rad/s) 


Software 
Measured Rate 
(rad/s, average) 


1 


6.0 


1.00000 


0.98174 


2 


3.5 


1.79485 


1.95667 


3 


2.19 


2.86849 


2.93160 


5 


1.69 


2.71716 


3.90653 


5.5 


No data 


No data 


4.88828 


10 


No data 


No data 


5.23598 


20 


No data 


No data 


5.23598 


30 


No data 


No data 


5.23598 



Figure A.l Inputs and results from massaged data (error). No data entries exist because the 
revolutions were too fast for hand timing. 



B. DESIRED INPUT RATE VS OUPUT FOR EACH WHEEL (SOFTWARE 
DEPENDENT 

Below M5, M6, M7, and M8 reperesent the steering motors for wheel 1, wheel 2, 
wheels, and wheel4 respectively. 



Desired Rate 
of turn 


M5 Rate 


M6 Rate 


M7 Rate 


M8 Rate 


1 


1.002 


.9975 


.9965 


.999 


2 


2.0025 


1.997 


1.990 


1.997 


3 


3.005 


2.995 


2.9975 


3.005 


4 


4.004 


3.996 


3.9925 


3.996 


5 


5.008 


5.002 


4.9985 


5.002 
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Desired Rate 
of turn 


M5 Rate 


M6 Rate 


M7 Rate 


M8 Rate 


5.1 


5.1035 


5.0935 


5.093 


5.0955 


5.2 


5.2065 


5.198 


5.1955 


5.198 


5.3 


5.238 


5.235 


5.234 


5.235 


5.4 


5.238 


5.235 


5.234 


5.235 


-1 


-1.001 


-1.002 


-1.002 


-1.002 


-2 


-2.001 


-2.003 


-2.003 


-2.004 


-3 


-3.0055 


-3.006 


-3.0055 


-3.006 


-4 


-4.007 


-4.006 


-4.0015 


-4.008 


-5 


-5.010 


-5.010 


-5.009 


-5.010 


-5.1 


-5.107 


-5.110 


-5.1035 


-5.112 


-5.2 


-5.2105 


-5.2125 


-5.2075 


-5.214 


-5.3 


-5.238 


-5.2415 


-5.237 


-5.2535 


-5.4 


-5.238 


-5.2415 


-5.237 


-5.2535 



Figure A.2: Desired (commanded) rate of turn vs. actual “free floating” motor rate. 



C. DESIRED INPUT (DIGIT MANIPULATION) VS OUTPUT RATE 



Input 


M5 Rate 


M6 Rate 


M7 Rate 


M8 Rate 


(digits) 


(rad/s) 


(rad/s) 


(rad/s) 


(rad/s) 


10 


.056 


.045 


.044 


.045 


20 


.102 


.101 


o 

o 

• 


.100 


30 


.152 


.143 


.143 


.143 


40 


.205 


.203 


.203 


.203 


50 


.261 


.250 


.248 


.249 


60 


.306 


.306 


.305 


.305 


70 


.363 


.350 


.349 


.350 


80 


.409 


.408 


.407 


.407 


90 


.466 


.454 


.452 


.455 


100 


.511 


.510 


.510 


.510 


200 


1.024 


1.022 


1.022 


1.022 
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Input 


M5 Rate 


M6 Rate 


M7 Rate 


M8 Rate 


(digits) 


(rad/s) 


(rad/s) 


(rad/s) 


(rad/s) 


300 


1.539 


1.533 


1.533 


1.533 


400 


2.049 


2.046 


2.045 


2.045 


500 


2.561 


2.556 


2.556 


2.556 


600 


3.074 


3.068 


3.067 


3.068 


700 


3.584 


3.579 


3.579 


3.579 


800 


4.097 


4.092 


4.091 


4.092 


900 


4.610 


4.602 


4.602 


4.602 


1000 


5.124 


5.116 


5.114 


5.116 


1010 


5.174 


5.172 


5.170 


5.172 


1020 


5.226 


5.218 


5.216 


5.218 


1021 


5.233 


5.226 


5.222 


5.225 


1022 


5.235 


5.233 


5.231 


5.232 


1023 


5.237 


5.235 


5.234 


5.235 



Figure A.3: Desired (commanded) rate of turn vs. actual “free floating” motor rate for each wheel 
using input digits . 



D. WHEEL 4 ROTATION DATA 





Clockwise Rotation 


Counterclockwise 

Rotation 


1 


000.867 


360.390 


2 


000.878 


360.390 


3 


000.976 


360.363 


4 


000.933 


360.414 


5 


000.984 


360.371 


6 


000.992 


360.394 


7 


000.992 


360.453 


8 


000.902 


360.394 


9 


000.996 


360.445 


10 


000.996 


360.476 


11 


001.003 


360.402 



289 





Clockwise Rotation 


Counterclockwise 

Rotation 


12 


000.917 


360.417 


13 


000.996 


360.433 


14 


001.003 


360.468 


15 


000.972 


360.468 


16 


000.863 


360.472 


17 


000.968 


360.398 


18 


000.941 


360.425 


19 


000.957 


360.480 


20 


000.937 


360.480 



Figure A.4: Wheel 4 data based on position of rest after direction of turn. 
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